def kalman_angle(): global kAngle_g T = 0.01 lamC = 1.0 #variance for compass lamGPS = 1.0 #variance for gps bearing lamGyro = 1.0 #variance for gyroscope sig = 1.0 #variance for model sig2 = sig**2 lamC2 = lamC**2 lamGPS2 = lamGPS**2 lamGyro2 = lamGyro**2 (A, H, Q, R) = create_model_parameters(T, sig2, lamC2) #initialize evolution, measurement, model error, and measurement error # initial state x = np.array([0, 0.1]) #starting velocity and position P = 0 * np.eye(2) #starting probability 100% kalman_filter = KalmanFilter(A, H, Q, R, x, P) #create the weights filter lastTime = time.time() startTime = lastTime lastOmega = 0 lastAngle = 0 thetaCoord_l = 0 #local variable to prevent race conditions lastBearing = 0 gpsTrust = 1.0 while True: while (omega_g == lastOmega or thetaCoord_g == lastAngle): #check if there were updates to the compass time.sleep(0.01) #if no updates then wait a bit thetaCoord_l = thetaCoord_g lastAngle = thetaCoord_l if (kalman_filter._x[0] - thetaCoord_l) > 180: #make sure the filter knows that we crossed the pole kalman_filter._x[0] = kalman_filter._x[0] - 360 elif (thetaCoord_l - kalman_filter._x[0]) > 180: kalman_filter._x[0] = kalman_filter._x[0] + 360 kalman_filter.predict() #evolve the state and the error kalman_filter.update((thetaCoord_l,omega_g),(lamC2,lamGyro2)) #load in 2 measurement values (x, P) = kalman_filter.get_state() #return state dt = time.time() - lastTime #calculate dt lastTime = time.time() kalman_filter.update_time(dt,sig2) #Make sure the filter knows how much time occured in between steps kAngle_g = x[0]%360 #because the model portion is estimating based on that. # while (omega_g == lastOmega or gpsTheta_g == lastBearing): #check if there were updates to the gps bearing # time.sleep(0.01) #if no updates then wait a bit gpsTheta_l = gpsTheta_g lastBearing = gpsTheta_l if (kalman_filter._x[0] - gpsTheta_l) > 180: #make sure the filter knows that we crossed the pole kalman_filter._x[0] = kalman_filter._x[0] - 360 elif (gpsTheta_l - kalman_filter._x[0]) > 180: kalman_filter._x[0] = kalman_filter._x[0] + 360 lamGPS2 = gpsTrust/gpsDistance_g kalman_filter.predict() #evolve the state and the error kalman_filter.update((gpsTheta_l,omega_g),(lamGPS2,lamGyro2)) #load in 2 measurement values (x, P) = kalman_filter.get_state() #return state dt = time.time() - lastTime #calculate dt lastTime = time.time() kalman_filter.update_time(dt,sig2) #Make sure the filter knows how much time occured in between steps kAngle_g = x[0]%360 #because the model portion is estimating based on that.
class TestKalmanFilter(unittest.TestCase): def setUp(self): A = np.eye(2) H = np.eye(2) Q = np.eye(2) R = np.eye(2) x_0 = np.array([1, 1]) P_0 = np.eye(2) self.kalman_filter = KalmanFilter(A, H, Q, R, x_0, P_0) def test_predict(self): self.kalman_filter.predict() (x, P) = self.kalman_filter.get_state() self.assertTrue((x == np.array([1, 1])).all()) self.assertTrue((P == 2 * np.eye(2)).all()) def test_update(self): self.kalman_filter.update(np.array([0, 0])) (x, P) = self.kalman_filter.get_state() self.assertTrue((x == np.array([0.5, 0.5])).all()) self.assertTrue((P == 0.5 * np.eye(2)).all())
np.random.seed(21) (A, H, Q, R) = create_model_parameters() K = 20 # initial state x = np.array([0, 0.5, 0, 0.5]) P = 0 * np.eye(4) (state, meas) = simulate_system(K, x) kalman_filter = KalmanFilter(A, H, Q, R, x, P) est_state = np.zeros((K, 4)) est_cov = np.zeros((K, 4, 4)) for k in range(K): kalman_filter.predict() kalman_filter.update(meas[k, :]) (x, P) = kalman_filter.get_state() est_state[k, :] = x est_cov[k, ...] = P plt.figure(figsize=(7, 5)) plt.plot(state[:, 0], state[:, 2], '-bo') plt.plot(est_state[:, 0], est_state[:, 2], '-ko') plt.plot(meas[:, 0], meas[:, 1], ':rs') plt.xlabel('x [m]') plt.ylabel('y [m]') plt.legend(['true state', 'inferred state', 'observed measurement']) plt.axis('square') plt.tight_layout(pad=0) plt.show()