def Filter(Z, T, k1, k2):
t = T[1] - T[0]
A = np.array([[1, 0, t, 0], [0, 1, 0, t], [0, 0, 1, 0], [0, 0, 0, 1]],
np.float32)
H = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
np.float32)
X = np.array([[np.float32(Z[0])], [np.float32(0)], [np.float32(0)],
[np.float32(0)]])
Q = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
np.float32) * k1
P = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
np.float32)
R = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
np.float32) * k2
new_z = []
new_z.append(Z[0])
kf = KalmanFilterLinear(A, H, X, P, Q, R)
size = len(Z)
for i in range(1, size):
current_pre = kf.GetCurrentState()
if i + 1 < size:
A[0][2] = A[1][3] = T[i + 1] - T[i]
#print A[0][2]
kf.Step(
np.array([[np.float32(Z[i])], [np.float32(0)], [np.float32(0)],
[np.float32(0)]]))
new_z.append(1.0 * current_pre[0][0] + Z[i - 1] * 0.0)
#print 'kalman'
return new_z
def test():
pylab.clf()
# sin(45)*100 = 70.710 and cos(45)*100 = 70.710
# vf = vo + at
# 0 = 70.710 + (-9.81)t
# t = 70.710/9.81 = 7.208 seconds for half
# 14.416 seconds for full journey
# distance = 70.710 m/s * 14.416 sec = 1019.36796 m
timeslice = 0.1 # How many seconds should elapse per iteration?
iterations = 144 # How many iterations should the simulation run for?
# (notice that the full journey takes 14.416 seconds, so 145 iterations will
# cover the whole thing when timeslice = 0.10)
noiselevel = 30 # How much noise should we add to the noisy measurements?
muzzle_velocity = 100 # How fast should the cannonball come out?
angle = 45 # Angle from the ground.
# These are arrays to store the data points we want to plot at the end.
x = []
y = []
nx = []
ny = []
kx = []
ky = []
# Let's make a cannon simulation.
c = Cannon(timeslice, noiselevel)
speedX = muzzle_velocity*math.cos(angle*math.pi/180)
speedY = muzzle_velocity*math.sin(angle*math.pi/180)
# This is the state transition vector, which represents part of the kinematics.
# 1, ts, 0, 0 => x(n+1) = x(n) + vx(n)
# 0, 1, 0, 0 => vx(n+1) = vx(n)
# 0, 0, 1, ts => y(n+1) = y(n) + vy(n)
# 0, 0, 0, 1 => vy(n+1) = vy(n)
# Remember, acceleration gets added to these at the control vector.
state_transition = numpy.matrix([[1, timeslice, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, timeslice],
[0, 0, 0, 1]])
control_matrix = numpy.matrix([[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
# The control vector, which adds acceleration to the kinematic equations.
# 0 => x(n+1) = x(n+1)
# 0 => vx(n+1) = vx(n+1)
# -9.81*ts^2 => y(n+1) = y(n+1) + 0.5*-9.81*ts^2
# -9.81*ts => vy(n+1) = vy(n+1) + -9.81*ts
control_vector = numpy.matrix([[0],
[0],
[0.5*-9.81*timeslice*timeslice],
[-9.81*timeslice]])
# After state transition and control, here are the equations:
# x(n+1) = x(n) + vx(n)
# vx(n+1) = vx(n)
# y(n+1) = y(n) + vy(n) - 0.5*9.81*ts^2
# vy(n+1) = vy(n) + -9.81*ts
# Which, if you recall, are the equations of motion for a parabola. Perfect.
# Observation matrix is the identity matrix, since we can get direct
# measurements of all values in our example.
observation_matrix = numpy.eye(4)
# This is our guess of the initial state. I intentionally set the Y value
# wrong to illustrate how fast the Kalman filter will pick up on that.
initial_state = numpy.matrix([[0],
[speedX],
[500],
[speedY]])
initial_probability = numpy.eye(4)
process_covariance = numpy.zeros(4)
measurement_covariance = numpy.eye(4)*0.2
kf = KalmanFilterLinear(state_transition,
control_matrix,
observation_matrix,
initial_state,
initial_probability,
process_covariance,
measurement_covariance)
# Iterate through the simulation.
for i in range(iterations):
x.append(c.GetX())
y.append(c.GetY())
newestX = c.GetXWithNoise()
newestY = c.GetYWithNoise()
nx.append(newestX)
ny.append(newestY)
# Iterate the cannon simulation to the next timeslice.
c.Step()
kx.append(kf.state_estimate[0, 0])
ky.append(kf.state_estimate[2, 0])
kf.update(control_vector,
numpy.matrix([
[newestX],
[c.GetXVelocity()],
[newestY],
[c.GetYVelocity()]
]))
# Plot all the results we got.
pylab.plot(x, y, '-',
nx, ny, ':',
kx, ky, '--')
pylab.xlabel('X position')
pylab.ylabel('Y position')
pylab.title('Measurement of a Cannonball in Flight')
pylab.legend(('true', 'measured', 'kalman'))
pylab.savefig("cannon.png")