daviddoji/Kalman-and-Bayesian-Filters-in-Python
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Switch to np.array.

Browse Source 
I switched all the equations to use np.array instead of np.matrx.

Also, I am starting to write the Kalman Math chapter in earnest.
This commit is contained in:
Roger Labbe 2014-08-22 07:37:47 -07:00
parent e5f968a2e3
commit 5590a57bb9
6 changed files with 815 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

367
Designing_Kalman_Filters.ipynb
View File

File diff suppressed because one or more lines are too long

436
Extended_Kalman_Filters.ipynb
View File

File diff suppressed because one or more lines are too long

80
Kalman_Filter_Math.ipynb
View File

@ -1,7 +1,7 @@
{
"metadata": {
"name": "",
"signature": "sha256:dd34f4987a60a8a3c453361b731ac3f9a06c88bce6e4f0c67907f60ea9ab99fc"
"signature": "sha256:e3462ab986f9dc643d523b8cb2a8312877c7122edff3d6d7accda49234359e80"
},
"nbformat": 3,
"nbformat_minor": 0,
@ -260,11 +260,68 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"This chapter is optional, especially the first time you go through this material. If you are a hobbiest you will be able to get quite far by just skipping this chapter and going on to learn the examples. However, to have any hope of reading the literature or to design more sophsticated kilters you will have to acquire some of the more formal mathematics. I will not endevour to teach linear algebra, statistics, or calculus here as each topic deserves a book on its own. \n",
"If you've gotten this far I hope that you are thinking that the Kalman filter's fearsome reputation is somewhat undeserved. Sure, I handwaved some equations away, but I hope implemention has been fairly straightforward for you. The underlying concept is quite straightforward - take two measurements, or a measurement and a prediction, and choose the output to be somewhere between the two. If you believe the measurement more your guess will be closer to the measurement, and if you believe the prediction is more accurate your guess will lie closer it it. That's not rocket science (little joke - it is exactly this math that got Apollo to the moon and back!). \n",
"\n",
"blah blah blah\n",
"Well, to be honest I have been choosing my problems carefully. For any arbitrary problem finding some of the matrices that we need to feed into the Kalman filter equations can be quite difficult. I haven't been *too tricky*, though. Equations like Newton's equations of motion can be trivially computed for Kalman filter applications, and they make up the bulk of the kind of problems that we want to solve. If you are a hobbiest, you can safely pass by this chapter for now, and perhaps forever. Some of the later chapters will assume the material in this chapter, but much of the work will still be acceessible to you. \n",
"\n",
"**authors note: need to start with continuous equations with $\\phi$, etc. Then discuss linearization into discrete case.**"
"But, I urge everyone to at least read the first section, and to skim the rest. It is not much harder than what you have done - the difficulty comes in finding closed form expressions for specific problems, not understanding the math in this chapter. \n"
]
},
{
"cell_type": "heading",
"level": 2,
"metadata": {},
"source": [
"Modelling a Linear System that Has Noise"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We need to start by understanding the underlying equations and assumptions that the Kalman filter uses. We are trying to model real world phenomena, so what do we have to consider?\n",
"\n",
"First, each physical system has a process. For example, a car travelling at a certain velocity goes so far in a fixed amount of time, and it's velocity varies as a function of it's aceleration. We describe that behavior with the well known Newtonian equations we learned in high school.\n",
"\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"v&=at\\\\\n",
"d &= \\frac{1}{2}at_2 + v_0t + d_0\n",
"\\end{aligned}\n",
"$$\n",
"\n",
"But, of course we know this is not all that is happening. First, we do not have perfect measures of things like the velocity and acceleration - there is always noise in the measurements, and we have to model that. Second, no car travels on a perfect road. There are bumps that cause the car to slow down, there is wind drag, there are hills that raise and lower the speed. If we do not have explicit knowledge of these factors we lump them all together under the term \"process noise\".\n",
"\n",
"Trying to model off of those factors explicitly and exactly is impossible for anything but the most trivial problem. I could try to modify Newton's equations for things like bumps in the road, the behavior of the car's suspension system, heck, the effects of hitting bugs with the windshield, but the job would never be done - there would always be more effects to add. What is worse, each of those models would in themselves be a simplification - do I assume the wind is constant, that the drag of the car is the same for all angles of the wind, that the suspension act as perfect springs, and so on?\n",
"\n",
"\n",
"So control theory makes a mathematically correct simplification. We acknowledge that there are many factors that influence the system that we either do not know or that we don't want to have to model. At any time $t$ we say that the actual value (say, the position of our car) is the predicted value plus some unknown process noise:\n",
"\n",
"$$\n",
"x(t) = x_{pred}(t) + noise(t)\n",
"$$\n",
"\n",
"This is not meant to imply that $noise(t)$ is a function that we can derive analytically or that it is well behaved. If there is a bump in the road at $t=10$ then the noise factor will just incorporate that effect. Again, this is not implying that we model, compute, or even know the value of *noise(t)*, it is merely a statement of fact - we can *always* describe the actual value as the predicted value plus some other value. \n",
"\n",
"Let's express this in linear algebra. Using the same notation from previous chapters, we can say that our model of the system (without noise) is:\n",
"\n",
"$$ f(x) = Fx$$\n",
"\n",
"That is, we have a set of linear equations that describe our system. For our car, \n",
"$F$ will be the coefficients for Newton's equations of motion. \n",
"\n",
"Now we need to model the noise. We will just call that *w*, and add it to the equation.\n",
"\n",
"$$ f(x) = Fx + w$$\n",
"\n",
"Finally, we need to consider inputs into the system. We are dealing with linear problems here, so we will assume that there is some input $u$ into the system, and that we have some linear model that defines how that input changes the system. For example, if you press down on the accelerator in your car the car will accelerate. We will need a matrix $G$ to convert $u$ into the effect on the system. We just add that into our equation:\n",
"\n",
"$$ f(x) = Fx + Gu + w$$\n",
"\n",
"And that's it. That is the equation that Kalman set out to solve, and he found a way to compute an optimal solution if we assume certain properties of $w$.\n",
"\n",
"\n"
]
},
{
@ -392,9 +449,20 @@
]
},
{
"cell_type": "markdown",
"cell_type": "heading",
"level": 2,
"metadata": {},
"source": []
"source": [
"Design of the Process Noise Matrix"
]
},
{
"cell_type": "code",
"collapsed": false,
"input": [],
"language": "python",
"metadata": {},
"outputs": []
}
],
"metadata": {}

38
ekf_internal.py
View File

@ -125,4 +125,40 @@ def plot_ball():
p2, = plt.plot (xs, ys,lw=2)
p3, = plt.plot (xs2, ys2,lw=4, c='r')
plt.legend([p1,p2, p3], ['Measurements', 'Kalman filter(R=0.5)', 'Kalman filter(R=10)'])
plt.show()
plt.show()
def show_radar_chart():
plt.xlim([0.9,2.5])
plt.ylim([0.5,2.5])
plt.scatter ([1,2],[1,2])
#plt.scatter ([2],[1],marker='o')
ax = plt.axes()
ax.annotate('', xy=(2,2), xytext=(1,1),
arrowprops=dict(arrowstyle='->', ec='r',shrinkA=3, shrinkB=4))
ax.annotate('', xy=(2,1), xytext=(1,1),
arrowprops=dict(arrowstyle='->', ec='b',shrinkA=0, shrinkB=0))
ax.annotate('', xy=(2,2), xytext=(2,1),
arrowprops=dict(arrowstyle='->', ec='b',shrinkA=0, shrinkB=4))
ax.annotate('Aircraft', xy=(2.04,2.), color='b')
ax.annotate('altitude', xy=(2.04,1.5), color='k')
ax.annotate('X', xy=(1.5, .9))
ax.annotate('Radar', xy=(.95, 0.9))
ax.annotate('Slant\n (r)', xy=(1.5,1.62), color='r')
plt.title("Radar Tracking")
ax.xaxis.set_ticklabels([])
ax.yaxis.set_ticklabels([])
ax.xaxis.set_ticks([])
ax.yaxis.set_ticks([])
plt.show()
show_radar_chart()

58
exp/RungeKutta.py
View File

@ -5,18 +5,44 @@ Created on Sat Jul 05 09:54:39 2014
@author: rlabbe
"""
from __future__ import division
from __future__ import division, print_function
import matplotlib.pyplot as plt
from scipy.integrate import ode
import math
import numpy as np
from numpy import random, radians, cos, sin
class BallTrajectory2D(object):
def __init__(self, x0, y0, velocity, theta_deg=0., g=9.8, noise=[0.0,0.0]):
theta = radians(theta_deg)
self.vx0 = velocity * cos(theta)
self.vy0 = velocity * sin(theta)
self.x0 = x0
self.y0 = y0
self.x = x
self.g = g
self.noise = noise
def position(self, t):
""" returns (x,y) tuple of ball position at time t"""
self.x = self.vx0*t + self.x0
self.y = -0.5*self.g*t**2 + self.vy0*t + self.y0
return (self.x +random.randn()*self.noise[0], self.y +random.randn()*self.noise[1])
class BallEuler(object):
def __init__(self, y=100., vel=10.):
def __init__(self, y=100., vel=10., omega=0):
self.x = 0.
self.y = y
self.vel = vel
self.y_vel = 0.0
omega = radians(omega)
self.vel = vel*np.cos(omega)
self.y_vel = vel*np.sin(omega)
@ -48,7 +74,7 @@ def rk4(y, x, dx, f):
k3 = dx * f(y + 0.5*k2, x + 0.5*dx)
k4 = dx * f(y + k3, x + dx)
return y + (k1 + 2*k2 + 2*k3 + k4) / 6
return y + (k1 + 2*k2 + 2*k3 + k4) / 6.
def rk2 (y,x,dx,f):
"""computes the 2nd order Runge-kutta for dy/dx"""
@ -78,8 +104,9 @@ class BallRungeKutta(object):
self.x = rk4 (self.x, self.t, dt, fx)
self.y = rk4 (self.y, self.t, dt, fy)
self.t += dt
print(fx.vel)
return (self.x, self.y)
def ball_scipy(y0, vel, omega, dt):
@ -104,22 +131,25 @@ if __name__ == "__main__":
dt = 1./30
y0 = 15.
vel = 100.
omega = 0.
omega = 30.
vel_y = math.sin(math.radians(omega)) * vel
def f(t,y):
return vel_y-9.8*t
be = BallEuler (y=y0, vel=vel)
be = BallEuler (y=y0, vel=vel,omega=omega)
#be = BallTrajectory2D (x0=0, y0=y0, velocity=vel, theta_deg = omega)
ball_rk = BallRungeKutta (y=y0, vel=vel, omega=omega)
while be.y >= 0:
be.step (dt)
ball_rk.step(dt)
print (ball_rk.y - be.y)
plt.scatter (be.x, be.y, color='red')
plt.scatter (ball_rk.x, ball_rk.y, color='blue', marker='v')
#plt.scatter (brk.x, y[0], color='green', marker='+')
#plt.axis('equal')
'''
p1 = plt.scatter (be.x, be.y, color='red')
p2 = plt.scatter (ball_rk.x, ball_rk.y, color='blue', marker='v')
plt.legend([p1,p2], ['euler', 'runge kutta'])
'''

5
stats.py
View File

@ -12,7 +12,8 @@ faster, at least on my machine. For example, gaussian average 794 ns, whereas
stats.norm(), using the frozen form, averages 116 microseconds per call.
"""
from __future__ import division
from __future__ import (absolute_import, division, print_function,
unicode_literals)
import math
import numpy as np
@ -252,4 +253,4 @@ if __name__ == '__main__':
plot_covariance_ellipse((2,7), cov=cov, variance=[1,2], title='my title')
plt.show()
print "all tests passed"
print("all tests passed")