Skip to content

Instantly share code, notes, and snippets.

@jaredly

jaredly/Document.nm

Created December 5, 2014 01:26
Show Gist options
  • Save jaredly/26d21ed5578073a01746 to your computer and use it in GitHub Desktop.
Save jaredly/26d21ed5578073a01746 to your computer and use it in GitHub Desktop.
Download ZIP
Kalman
Raw
Document.nm
{
"created": 1417740315890,
"modified": 1417742789938,
"content": "Kalman",
"children": [
{
"content": "import numpy",
"children": [],
"modified": 1417740315958,
"language": "python",
"type": "ipython",
"waiting": false,
"started": 1417740333135,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "import numpy",
"finished": 1417740333186,
"display_collapsed": false,
"outputs": []
},
{
"created": 1417740415745,
"modified": 1417740608903,
"content": "def make_F(t, c=0):\n return numpy.matrix([\n [1, t, t**2/2.0, 0, 0, 0],\n [0, 1, t, 0, 0, 0],\n [0, -c, 1, 0, 0, 0],\n [0, 0, 0, 1, t, t**2/2.0],\n [0, 0, 0, 0, 1, t],\n [0, 0, 0, 0, -c, 1]\n ])",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417740608920,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "def make_F(t, c=0):\n return numpy.matrix([\n [1, t, t**2/2.0, 0, 0, 0],\n [0, 1, t, 0, 0, 0],\n [0, -c, 1, 0, 0, 0],\n [0, 0, 0, 1, t, t**2/2.0],\n [0, 0, 0, 0, 1, t],\n [0, 0, 0, 0, -c, 1]\n ])",
"finished": 1417740608920,
"display_collapsed": false,
"outputs": []
},
{
"created": 1417740572359,
"modified": 1417740609670,
"content": "print make_F(10)",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417740609670,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "print make_F(10)",
"finished": 1417740609671,
"display_collapsed": false,
"outputs": [
{
"type": "stream",
"stream": "stdout",
"text": "[[ 1. 10. 50. 0. 0. 0.]\n [ 0. 1. 10. 0. 0. 0.]\n [ 0. 0. 1. 0. 0. 0.]\n [ 0. 0. 0. 1. 10. 50.]\n [ 0. 0. 0. 0. 1. 10.]\n [ 0. 0. 0. 0. 0. 1.]]\n"
}
]
},
{
"created": 1417740643582,
"modified": 1417740670256,
"content": "zigma_X = numpy.matrix([[0.1,0,0,0,0,0],\n[0,0.1,0,0,0,0],\n[0,0,100,0,0,0],\n[0,0,0,0.1,0,0],\n[0,0,0,0,0.1,0],\n[0,0,0,0,0,100]])\nprint zigma_X",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417740670256,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "zigma_X = numpy.matrix([[0.1,0,0,0,0,0],\n[0,0.1,0,0,0,0],\n[0,0,100,0,0,0],\n[0,0,0,0.1,0,0],\n[0,0,0,0,0.1,0],\n[0,0,0,0,0,100]])\nprint zigma_X",
"finished": 1417740670256,
"display_collapsed": false,
"outputs": [
{
"type": "stream",
"stream": "stdout",
"text": "[[ 0.1 0. 0. 0. 0. 0. ]\n [ 0. 0.1 0. 0. 0. 0. ]\n [ 0. 0. 100. 0. 0. 0. ]\n [ 0. 0. 0. 0.1 0. 0. ]\n [ 0. 0. 0. 0. 0.1 0. ]\n [ 0. 0. 0. 0. 0. 100. ]]\n"
}
]
},
{
"created": 1417740694054,
"modified": 1417740709927,
"content": "H = numpy.matrix([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])\nprint H",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417740709927,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "H = numpy.matrix([[1, 0, 0, 0, 0, 0], [0, 0, 0, 1, 0, 0]])\nprint H",
"finished": 1417740709927,
"display_collapsed": false,
"outputs": [
{
"type": "stream",
"stream": "stdout",
"text": "[[1 0 0 0 0 0]\n [0 0 0 1 0 0]]\n"
}
]
},
{
"created": 1417741431143,
"modified": 1417741610565,
"content": "np = numpy",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417741435448,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "np = numpy",
"finished": 1417741435448,
"display_collapsed": false,
"outputs": []
},
{
"created": 1417740716821,
"modified": 1417740752485,
"content": "def make_zZ(noise):\n return numpy.matrix([[noise**2, 0], [0, noise**2]])",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417740752493,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "def make_zZ(noise):\n return numpy.matrix([[noise**2, 0], [0, noise**2]])",
"finished": 1417740752493,
"display_collapsed": false,
"outputs": []
},
{
"created": 1417740753445,
"modified": 1417740765054,
"content": "print make_zZ(5)",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417740765053,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "print make_zZ(5)",
"finished": 1417740765054,
"display_collapsed": false,
"outputs": [
{
"type": "stream",
"stream": "stdout",
"text": "[[25 0]\n [ 0 25]]\n"
}
]
},
{
"content": "mu = numpy.matrix([0]*6).T",
"children": [],
"created": 1417740354361,
"modified": 1417740867389,
"type": "ipython",
"language": "python",
"waiting": false,
"started": 1417740373488,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "mu = numpy.matrix([0]*6).T",
"finished": 1417740373490,
"display_collapsed": false,
"outputs": []
},
{
"created": 1417740887564,
"modified": 1417741606861,
"content": "def go_big_or_go_home(F, St, Sx):\n return (F * St * F.T + Sx)\n\ndef Kplus1(F, St, Sx, H, Sz, BIG):\n return BIG * H.T * (H * BIG * H.T + Sz) - 1\n\ndef muplus1(F, mu, Knext, Znext, H):\n efmu = F * mu\n return efmu + Knext * (Znext - H * efmu)\n\ndef Zigmaplus1(Knext, H, BIG):\n return (np.identity(6) - Knext * H) * BIG",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417741612260,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "def go_big_or_go_home(F, St, Sx):\n return (F * St * F.T + Sx)\n\ndef Kplus1(F, St, Sx, H, Sz, BIG):\n return BIG * H.T * (H * BIG * H.T + Sz) - 1\n\ndef muplus1(F, mu, Knext, Znext, H):\n efmu = F * mu\n return efmu + Knext * (Znext - H * efmu)\n\ndef Zigmaplus1(Knext, H, BIG):\n return (np.identity(6) - Knext * H) * BIG",
"finished": 1417741612260,
"display_collapsed": false,
"outputs": []
},
{
"created": 1417741615725,
"modified": 1417742767898,
"content": "class Kalman:\n H = numpy.matrix([[1, 0, 0, 0, 0, 0], \n [0, 0, 0, 1, 0, 0]])\n Ht = H.T\n Sx = numpy.matrix([\n [0.1,0, 0, 0, 0, 0],\n [0, 0.1,0, 0, 0, 0],\n [0, 0, 100,0, 0, 0],\n [0, 0, 0, 0.1,0, 0],\n [0, 0, 0, 0, 0.1,0],\n [0, 0, 0, 0, 0, 100]])\n def __init__(self, noise, timedelta, initial_pos):\n self.noise = noise\n self.timedelta = timedelta\n self._make_constants()\n self._make_initials(initial_pos)\n \n def _make_constants(self):\n self.Sz = make_zZ(self.noise)\n self.F = make_F(self.timedelta)\n self.Ft = self.F.T\n \n def _make_initials(self, (x, y)):\n self.mu = np.matrix(np.zeros((6,1)))\n self.mu[0, 0] = x\n self.mu[3, 0] = y\n self.St = np.matrix(np.zeros((6,6)))\n self.St[0, 0] = self.St[3, 3] = 100\n self.St[1, 1] = self.St[2, 2] = self.St[4, 4] = self.St[5, 5] = .1\n \n def update(self, x, y):\n '''takes an observed x, y and returns\n ((x, y), confX, confY)\n '''\n Znext = np.matrix([[x], [y]])\n BIG = go_big_or_go_home(self.F, self.St, self.Sx)\n K1 = Kplus1(self.F, self.St, self.Sx, self.H, self.Sz, BIG)\n mu1 = muplus1(self.F, self.mu, K1, Znext, self.H)\n S1 = Zigmaplus1(K1, self.H, BIG)\n \n self.mu = mu1\n self.St = S1\n \n return (self.H * mu1).T.tolist()[0], S1[0,0], S1[3,3]",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417742768240,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "class Kalman:\n H = numpy.matrix([[1, 0, 0, 0, 0, 0], \n [0, 0, 0, 1, 0, 0]])\n Ht = H.T\n Sx = numpy.matrix([\n [0.1,0, 0, 0, 0, 0],\n [0, 0.1,0, 0, 0, 0],\n [0, 0, 100,0, 0, 0],\n [0, 0, 0, 0.1,0, 0],\n [0, 0, 0, 0, 0.1,0],\n [0, 0, 0, 0, 0, 100]])\n def __init__(self, noise, timedelta, initial_pos):\n self.noise = noise\n self.timedelta = timedelta\n self._make_constants()\n self._make_initials(initial_pos)\n \n def _make_constants(self):\n self.Sz = make_zZ(self.noise)\n self.F = make_F(self.timedelta)\n self.Ft = self.F.T\n \n def _make_initials(self, (x, y)):\n self.mu = np.matrix(np.zeros((6,1)))\n self.mu[0, 0] = x\n self.mu[3, 0] = y\n self.St = np.matrix(np.zeros((6,6)))\n self.St[0, 0] = self.St[3, 3] = 100\n self.St[1, 1] = self.St[2, 2] = self.St[4, 4] = self.St[5, 5] = .1\n \n def update(self, x, y):\n '''takes an observed x, y and returns\n ((x, y), confX, confY)\n '''\n Znext = np.matrix([[x], [y]])\n BIG = go_big_or_go_home(self.F, self.St, self.Sx)\n K1 = Kplus1(self.F, self.St, self.Sx, self.H, self.Sz, BIG)\n mu1 = muplus1(self.F, self.mu, K1, Znext, self.H)\n S1 = Zigmaplus1(K1, self.H, BIG)\n \n self.mu = mu1\n self.St = S1\n \n return (self.H * mu1).T.tolist()[0], S1[0,0], S1[3,3]",
"finished": 1417742768266,
"display_collapsed": false,
"outputs": []
},
{
"created": 1417742699875,
"modified": 1417742711295,
"content": "k = Kalman(5, .5, (0, 0))",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417742769542,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "k = Kalman(5, .5, (0, 0))",
"finished": 1417742769551,
"display_collapsed": false,
"outputs": []
},
{
"created": 1417742738818,
"modified": 1417742773139,
"content": "k.update(100, 100)",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417742773139,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "k.update(100, 100)",
"finished": 1417742773140,
"display_collapsed": false,
"outputs": [
{
"text/plain": "([-4.5991665391907596e+164, -4.5991665391907596e+164],\n 4.6298276416362685e+164,\n 4.6298276416362685e+164)",
"type": "output"
}
]
},
{
"created": 1417741901899,
"modified": 1417741916242,
"content": "np.matrix(np.zeros((6,1)))",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417741910136,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "np.zeros((6,1))",
"finished": 1417741910136,
"display_collapsed": false,
"outputs": [
{
"text/plain": "array([[ 0.],\n [ 0.],\n [ 0.],\n [ 0.],\n [ 0.],\n [ 0.]])",
"type": "output"
}
]
},
{
"created": 1417742342782,
"modified": 1417742594661,
"content": "print mu.T.tolist()[0]",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417742594660,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "print mu.T.tolist()[0]",
"finished": 1417742594661,
"display_collapsed": false,
"outputs": [
{
"type": "stream",
"stream": "stdout",
"text": "[0, 0, 0, 2, 0, 0]\n"
}
]
},
{
"created": 1417742354070,
"modified": 1417742361854,
"content": "mu[3,0] = 2",
"type": "ipython",
"children": [],
"language": "python",
"waiting": false,
"started": 1417742361861,
"session": "18f90b4e-5edc-49d8-b7b0-4a39ffe67541ksy2ir283vlxmv6whio1e9ou92bhhyaj",
"executed": "mu[3,0] = 2",
"finished": 1417742361862,
"display_collapsed": false,
"outputs": []
}
]
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment