Kalman-and-Bayesian-Filters-in-Python/experiments/satellite.ipynb

{
 "metadata": {
  "name": "",
  "signature": "sha256:9fb5a469b0b0f27f1e721735fae76d74f0ad242621b94c3f528e9392b7deab82"
 },
 "nbformat": 3,
 "nbformat_minor": 0,
 "worksheets": [
  {
   "cells": [
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "import math\n",
      "\n",
      "def range_to(x1, x2):\n",
      "    dx = x1[0] - x2[0]\n",
      "    dy = x1[1] - x2[1]\n",
      "    print('dx',dx,dy)\n",
      "    return math.sqrt(dx*dx + dy*dy)"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [],
     "prompt_number": 40
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "s1 = (0., 2.) # satellite 1\n",
      "s2 = (1., 2.) # satellite 2\n",
      "p = (-0.5, 1.) # platform postion\n",
      "\n",
      "\n",
      "s1 = (0., 200.) # satellite 1\n",
      "s2 = (100., 220.) # satellite 2\n",
      "p = (10, 1.) # platform postion\n",
      "\n",
      "r1 = range_to(s1, p)\n",
      "r2 = range_to(s2, p)\n",
      "\n",
      "x1, y1 = s1[0], s1[1]\n",
      "x2, y2 = s2[0], s2[1]\n",
      "\n",
      "A = -(y2 - y1) / (x2 - x1)\n",
      "B = (r1**2 - r2**2 - x1**2 - y1**2 + x2**2 + y2**2) / (2*(x2 - x1))\n",
      "\n",
      "a = 1 + A**2\n",
      "b = -2*A*x1 + 2*A*B - 2*y1\n",
      "c = x1**2 - 2*x1*B + y1**2 - r1**2\n",
      "\n",
      "y = (-b - math.sqrt(b**2 - 4*a*c)) / (2*a)\n",
      "x = A*y + B\n",
      "\n",
      "print('p', x,y)\n",
      "print('err', x-p[0], y-p[1])"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "output_type": "stream",
       "stream": "stdout",
       "text": [
        "dx -10.0 199.0\n",
        "dx 90.0 219.0\n",
        "p 10.051726583768032 0.7413670811596572\n",
        "err 0.05172658376803163 -0.2586329188403428\n"
       ]
      }
     ],
     "prompt_number": 55
    },
    {
     "cell_type": "code",
     "collapsed": false,
     "input": [
      "A"
     ],
     "language": "python",
     "metadata": {},
     "outputs": [
      {
       "metadata": {},
       "output_type": "pyout",
       "prompt_number": 56,
       "text": [
        "-0.2"
       ]
      }
     ],
     "prompt_number": 56
    }
   ],
   "metadata": {}
  }
 ]
}