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")