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