Started altering to use filterpy project.
Not fully tested, but the multidimensional chapter is working.
This commit is contained in:
Roger Labbe
@@ -1,131 +0,0 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Created on Fri Oct 18 18:02:07 2013
|
||||
|
||||
@author: rlabbe
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
import scipy.linalg as linalg
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy.random as random
|
||||
|
||||
class KalmanFilter:
|
||||
|
||||
def __init__(self, dim_x, dim_z, use_short_form=False):
|
||||
""" Create a Kalman filter of dimension 'dim', where dimension is the
|
||||
number of state variables.
|
||||
|
||||
use_short_form will force the filter to use the short form equation
|
||||
for computing covariance: (I-KH)P. This is the equation that results
|
||||
from deriving the Kalman filter equations. It is efficient to compute
|
||||
but not numerically stable for very long runs. The long form,
|
||||
implemented in update(), is very slightly suboptimal, and takes longer
|
||||
to compute, but is stable. I have chosen to make the long form the
|
||||
default behavior. If you really want the short form, either call
|
||||
update_short_form() directly, or set 'use_short_form' to true. This
|
||||
causes update() to use the short form. So, if you do it this way
|
||||
you will never be able to use the long form again with this object.
|
||||
"""
|
||||
|
||||
self.x = 0 # state
|
||||
self.P = np.matrix(np.eye(dim_x)) # uncertainty covariance
|
||||
self.Q = np.matrix(np.eye(dim_x)) # process uncertainty
|
||||
self.u = np.matrix(np.zeros((dim_x,1))) # motion vector
|
||||
self.B = 0
|
||||
self.F = 0 # state transition matrix
|
||||
self.H = 0 # Measurement function (maps state to measurements)
|
||||
self.R = np.matrix(np.eye(dim_z)) # state uncertainty
|
||||
self.I = np.matrix(np.eye(dim_x))
|
||||
|
||||
if use_short_form:
|
||||
self.update = self.update_short_form
|
||||
|
||||
|
||||
def update(self, Z):
|
||||
"""
|
||||
Add a new measurement to the kalman filter.
|
||||
"""
|
||||
|
||||
# measurement update
|
||||
y = Z - (self.H * self.x) # error (residual) between measurement and prediction
|
||||
S = (self.H * self.P * self.H.T) + self.R # project system uncertainty into measurment space + measurement noise(R)
|
||||
|
||||
|
||||
K = self.P * self.H.T * linalg.inv(S) # map system uncertainty into kalman gain
|
||||
|
||||
self.x = self.x + (K*y) # predict new x with residual scaled by the kalman gain
|
||||
|
||||
KH = K*self.H
|
||||
I_KH = self.I - KH
|
||||
self.P = I_KH*self.P * I_KH.T + K*self.R*K.T
|
||||
|
||||
|
||||
def update_short_form(self, Z):
|
||||
"""
|
||||
Add a new measurement to the kalman filter.
|
||||
|
||||
Uses the 'short form' computation for P, which is mathematically
|
||||
correct, but perhaps not that stable when dealing with large data
|
||||
sets. But, it is fast to compute. Advice varies; some say to never
|
||||
use this. My opinion - if the longer form in update() executes too
|
||||
slowly to run in real time, what choice do you have. But that should
|
||||
be a rare case, so the long form is the default use
|
||||
"""
|
||||
|
||||
# measurement update
|
||||
y = Z - (self.H * self.x) # error (residual) between measurement and prediction
|
||||
S = (self.H * self.P * self.H.T) + self.R # project system uncertainty into measurment space + measurement noise(R)
|
||||
|
||||
|
||||
K = self.P * self.H.T * linalg.inv(S) # map system uncertainty into kalman gain
|
||||
|
||||
self.x = self.x + (K*y) # predict new x with residual scaled by the kalman gain
|
||||
|
||||
# and compute the new covariance
|
||||
self.P = (self.I - (K*self.H))*self.P # and compute the new covariance
|
||||
|
||||
|
||||
|
||||
def predict(self):
|
||||
# prediction
|
||||
self.x = (self.F*self.x) + self.B * self.u
|
||||
self.P = self.F * self.P * self.F.T + self.Q
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
f = KalmanFilter (dim_x=2, dim_z=2)
|
||||
|
||||
f.x = np.matrix([[2.],
|
||||
[0.]]) # initial state (location and velocity)
|
||||
|
||||
f.F = np.matrix([[1.,1.],
|
||||
[0.,1.]]) # state transition matrix
|
||||
|
||||
f.H = np.matrix([[1.,0.]]) # Measurement function
|
||||
f.P *= 1000. # covariance matrix
|
||||
f.R = 5 # state uncertainty
|
||||
f.Q *= 0.0001 # process uncertainty
|
||||
|
||||
measurements = []
|
||||
results = []
|
||||
for t in range (100):
|
||||
# create measurement = t plus white noise
|
||||
z = t + random.randn()*20
|
||||
|
||||
# perform kalman filtering
|
||||
f.measure (z)
|
||||
f.predict()
|
||||
|
||||
# save data
|
||||
results.append (f.x[0,0])
|
||||
measurements.append(z)
|
||||
|
||||
# plot data
|
||||
p1, = plt.plot(measurements,'r')
|
||||
p2, = plt.plot (results,'b')
|
||||
p3, = plt.plot ([0,100],[0,100], 'g') # perfect result
|
||||
plt.legend([p1,p2, p3], ["noisy measurement", "KF output", "ideal"], 4)
|
||||
|
||||
|
||||
plt.show()
|
||||
108
exp/dme.py
Normal file
108
exp/dme.py
Normal file
@@ -0,0 +1,108 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Spyder Editor
|
||||
|
||||
This is a temporary script file.
|
||||
"""
|
||||
|
||||
from KalmanFilter import *
|
||||
from math import cos, sin, sqrt, atan2
|
||||
|
||||
|
||||
def H_of (pos, pos_A, pos_B):
|
||||
""" Given the position of our object at 'pos' in 2D, and two transmitters
|
||||
A and B at positions 'pos_A' and 'pos_B', return the partial derivative
|
||||
of H
|
||||
"""
|
||||
|
||||
theta_a = atan2(pos_a[1]-pos[1], pos_a[0] - pos[0])
|
||||
theta_b = atan2(pos_b[1]-pos[1], pos_b[0] - pos[0])
|
||||
|
||||
if False:
|
||||
return np.mat([[0, -cos(theta_a), 0, -sin(theta_a)],
|
||||
[0, -cos(theta_b), 0, -sin(theta_b)]])
|
||||
else:
|
||||
return np.mat([[-cos(theta_a), 0, -sin(theta_a), 0],
|
||||
[-cos(theta_b), 0, -sin(theta_b), 0]])
|
||||
|
||||
class DMESensor(object):
|
||||
def __init__(self, pos_a, pos_b, noise_factor=1.0):
|
||||
self.A = pos_a
|
||||
self.B = pos_b
|
||||
self.noise_factor = noise_factor
|
||||
|
||||
def range_of (self, pos):
|
||||
""" returns tuple containing noisy range data to A and B
|
||||
given a position 'pos'
|
||||
"""
|
||||
|
||||
ra = sqrt((self.A[0] - pos[0])**2 + (self.A[1] - pos[1])**2)
|
||||
rb = sqrt((self.B[0] - pos[0])**2 + (self.B[1] - pos[1])**2)
|
||||
|
||||
return (ra + random.randn()*self.noise_factor,
|
||||
rb + random.randn()*self.noise_factor)
|
||||
|
||||
|
||||
pos_a = (100,-20)
|
||||
pos_b = (-100, -20)
|
||||
|
||||
f1 = KalmanFilter(dim_x=4, dim_z=2)
|
||||
|
||||
f1.F = np.mat ([[0, 1, 0, 0],
|
||||
[0, 0, 0, 0],
|
||||
[0, 0, 0, 1],
|
||||
[0, 0, 0, 0]])
|
||||
|
||||
f1.B = 0.
|
||||
|
||||
f1.R *= 1.
|
||||
f1.Q *= .1
|
||||
|
||||
f1.x = np.mat([1,0,1,0]).T
|
||||
f1.P = np.eye(4) * 5.
|
||||
|
||||
# initialize storage and other variables for the run
|
||||
count = 30
|
||||
xs, ys = [],[]
|
||||
pxs, pys = [],[]
|
||||
|
||||
# create the simulated sensor
|
||||
d = DMESensor (pos_a, pos_b, noise_factor=1.)
|
||||
|
||||
# pos will contain our nominal position since the filter does not
|
||||
# maintain position.
|
||||
pos = [0,0]
|
||||
|
||||
for i in range(count):
|
||||
# move (1,1) each step, so just use i
|
||||
pos = [i,i]
|
||||
|
||||
# compute the difference in range between the nominal track and measured
|
||||
# ranges
|
||||
ra,rb = d.range_of(pos)
|
||||
rx,ry = d.range_of((i+f1.x[0,0], i+f1.x[2,0]))
|
||||
|
||||
print ra, rb
|
||||
print rx,ry
|
||||
z = np.mat([[ra-rx],[rb-ry]])
|
||||
print z.T
|
||||
|
||||
# compute linearized H for this time step
|
||||
f1.H = H_of (pos, pos_a, pos_b)
|
||||
|
||||
# store stuff so we can plot it later
|
||||
xs.append (f1.x[0,0]+i)
|
||||
ys.append (f1.x[2,0]+i)
|
||||
pxs.append (pos[0])
|
||||
pys.append(pos[1])
|
||||
|
||||
# perform the Kalman filter steps
|
||||
f1.predict ()
|
||||
f1.update(z)
|
||||
|
||||
|
||||
p1, = plt.plot (xs, ys, 'r--')
|
||||
p2, = plt.plot (pxs, pys)
|
||||
plt.legend([p1,p2], ['filter', 'ideal'], 2)
|
||||
plt.show()
|
||||
|
||||
9
exp/temp.py
Normal file
9
exp/temp.py
Normal file
@@ -0,0 +1,9 @@
|
||||
# -*- coding: utf-8 -*-
|
||||
"""
|
||||
Spyder Editor
|
||||
|
||||
This is a temporary script file.
|
||||
"""
|
||||
|
||||
import KalmanFilter
|
||||
|
||||
Reference in New Issue
Block a user