# Extended Kalman filter + IMU sensor

I'm trying to implement an extended Kalman filter to the accelerometer and gyroscope. However, the result is not good. It seems to have some errors in my codes. I check the equation and code again but I don't find the errors.

''' import serial import numpy as np import math import time

np.set_printoptions(precision=3, suppress=True)

class ExtendedKalmanFilter(object): def init(self, x0, P0, Q, R): # initialize vectors and matrices self.x0 = x0 self.P0 = P0 self.Q = Q self.R = R #self.dT = dT self.currentTimeStep = 0 self.estimates_aposteriori = [] self.estimates_aposteriori.append(x0) self.estimates_apriori = [] self.estimationErrorCovarianceMatricesAposteriori = [] self.estimationErrorCovarianceMatricesAposteriori.append(P0) self.estimationErrorCovarianceMatricesApriori = [] self.gainMatrices = [] self.errors = []

def stateSpaceContinuous(self, gyro_raw, x_k):
transition_matrix = np.zeros(shape=(2, 3))
transition_matrix[0, 0] = 1
transition_matrix[0, 1] = np.sin(x_k[0, 0]) * np.tan(x_k[1, 0])
transition_matrix[0, 2] = np.cos(x_k[0, 0]) * np.tan(x_k[1, 0])
transition_matrix[1, 0] = 0
transition_matrix[1, 1] = np.cos(x_k[0, 0])
transition_matrix[1, 2] = -np.sin(x_k[0, 0])

dxdt = np.matmul(transition_matrix, gyro_raw)
return dxdt

def discreteTimeDynamics(self, gyro_raw, x_k, dT):
x_kp1 = x_k + dT * self.stateSpaceContinuous(gyro_raw, x_k)
return x_kp1

def jacobianStateEquation(self, gyro_raw, x):
p = gyro_raw[0, 0]
q = gyro_raw[1, 0]
r = gyro_raw[2, 0]
A = np.zeros(shape=(2, 2))
A[0, 0] = q * np.cos(x[0, 0]) * np.tan(x[1, 0]) - r * np.sin(x[0, 0]) * np.tan(x[1, 0])
A[0, 1] = (q * np.sin(x[0, 0]) * 1 / np.square(np.cos(x[1, 0]))) + (r * np.cos(x[0, 0]) * 1 / np.square(np.cos(x[1, 0])))
A[1, 0] = -q * np.sin(x[0, 0]) - r * np.cos(x[0, 0])
A[1, 1] = 0
return A

def jacobianOutputEquation(self, x):
C = np.zeros(shape=(2, 2))
C[0, 0] = 0
C[0, 1] = np.cos(x[1, 0])
C[1, 0] = -np.cos(x[1, 0]) * np.cos(x[0, 0])
C[1, 1] = np.sin(x[1, 0]) * np.sin(x[0, 0])
#C[2, 0] = np.cos(x[1, 0]) * np.sin(x[0, 0])
#C[2, 1] = np.sin(x[1, 0]) * np.cos(x[0, 0])
return C

def outputEquation(self, x_k):
return x_k

def propagateDynamics(self, gyro_raw, dt):
# propagate the a posteriori estimate to compute the a priori estimate
xk_minus = self.discreteTimeDynamics(gyro_raw, self.estimates_aposteriori[self.currentTimeStep], dt)
#print("xk minus",xk_minus)
# linearize the dynamics at the a posteriori estimate
Akm1 = self.jacobianStateEquation(gyro_raw, self.estimates_aposteriori[self.currentTimeStep])
#print("Akm1",Akm1.shape)
# propagate the a posteriori covariance matrix in time to compute the a priori covariance
Pk_minus = np.matmul(np.matmul(Akm1, self.estimationErrorCovarianceMatricesAposteriori[self.currentTimeStep]),
Akm1.T) + self.Q
#print("Pk minus",Pk_minus.shape)

# memorize the computed values and increment the time step
self.estimates_apriori.append(xk_minus)
self.estimationErrorCovarianceMatricesApriori.append(Pk_minus)
self.currentTimeStep = self.currentTimeStep + 1

def computeAposterioriEstimate(self, currentMeasurement):
currentMeasurement_angle = np.zeros(shape=(2, 1))
currentMeasurement_angle[0,0] = math.atan(currentMeasurement[1,0] / math.sqrt(currentMeasurement[0,0]**2 + currentMeasurement[2,0]**2))
currentMeasurement_angle[1,0] = -math.atan(currentMeasurement[0,0] / math.sqrt(currentMeasurement[1,0]**2 + currentMeasurement[2,0]**2))
#print(currentMeasurement_angle.shape)

# linearize the output equation at the a priori estimate for the time step k
Ck = self.jacobianOutputEquation(self.estimates_apriori[self.currentTimeStep - 1])
#print("Ck",Ck.shape)

Smatrix = self.R + np.matmul(
np.matmul(Ck, self.estimationErrorCovarianceMatricesApriori[self.currentTimeStep - 1]), Ck.T)
#print("Smatrix",Smatrix.shape)
# Kalman gain matrix
Kk = np.matmul(self.estimationErrorCovarianceMatricesApriori[self.currentTimeStep - 1],
np.matmul(Ck.T, np.linalg.inv(Smatrix)))
#print("Kk",Kk.shape)

# update the estimate
# prediction error
sensor_model = np.zeros(shape=(2, 1))
sensor_model[0,0] = np.sin(self.estimates_apriori[self.currentTimeStep - 1][1,0])
sensor_model[1,0] = -np.cos(self.estimates_apriori[self.currentTimeStep - 1][1,0])*np.sin(self.estimates_apriori[self.currentTimeStep - 1][0,0])
error_k = currentMeasurement_angle - sensor_model
#error_k = currentMeasurement_angle - self.outputEquation(self.estimates_apriori[self.currentTimeStep - 1])
#print("error k",error_k.shape)
# a posteriori estimate
#print('P danh gia',self.estimates_apriori[self.currentTimeStep - 1].shape)
xk_plus = self.estimates_apriori[self.currentTimeStep - 1] + np.matmul(Kk, np.array([error_k])).reshape(2,1)
#print('xk plus',xk_plus.shape)

# update the covariance matrix
# a posteriori covariance matrix update
IminusKkC = np.eye(2,2)
#print("Iminus",IminusKkC.shape)
#print(np.matmul(self.R,Kk.T).shape)
#Pk_plus = np.matmul(IminusKkC,np.matmul(self.estimationErrorCovarianceMatricesApriori[self.currentTimeStep - 1],
#IminusKkC.T)) + np.matmul(Kk, np.matmul(self.R, Kk.T))
Pk_plus = np.matmul((IminusKkC-np.matmul(Kk,Ck)),self.estimationErrorCovarianceMatricesApriori[self.currentTimeStep - 1])
#print(Pk_plus.shape)

# update the lists that store the vectors and matrices
# Kalman gain matrix
self.gainMatrices.append(Kk)
# errors
self.errors.append(error_k)
# a posteriori estimates
self.estimates_aposteriori.append(xk_plus)
# a posteriori covariance matrix
self.estimationErrorCovarianceMatricesAposteriori.append(Pk_plus)


############################################################

### Get the raw data

ser = serial.Serial('COM3', 115200, timeout=1)

acc = [] gyro = [] mag = [] old_time = time.time() ###############

x0 = np.array([np.pi/3, np.pi/3]).reshape(2, 1)

x0guess = np.zeros(shape=(2, 1)) x0guess[0] = x0[0] + 4 * np.random.randn() x0guess[1] = x0[1] + 4 * np.random.randn()

P0 = np.eye(2, 2) P0[0,0] = 0.1 P0[0,1] = 0 P0[1,0] = 0 P0[1,1] = 0.1

Q = np.eye(2, 2)

R = np.array([[1,0],[0,1]]).reshape(2,2)

KalmanFilterObject = ExtendedKalmanFilter(x0guess, P0, Q, R) i = 0 while True: data = ser.readline().decode("utf-8") dt = time.time() - old_time old_time = time.time() data = data[:-2] data = data.split(',') if data[0] == "\$RAW": acc = np.array(data[1:4], dtype=float).reshape(3, 1) gyro = np.array(data[4:7], dtype=float).reshape(3, 1) mag = data[7:10] # phi, theta = prediction(initial_state_estimate, gyro, sensor_noise_w_k, dt, 0, 0) # print(phi, theta) #################################################################################

    KalmanFilterObject.propagateDynamics(gyro, dt)
KalmanFilterObject.computeAposterioriEstimate(acc)
print(KalmanFilterObject.estimates_aposteriori[-1]*(180/np.pi))
#print("tien nghiem",KalmanFilterObject.estimates_apriori[-1])

# i += 1
# if i == 1000:
#     break
#############################################################


'''

• Welcome to Robotics David, but I'm afraid that it is not clear what you are asking. We prefer practical, answerable questions, so it's a good idea to include details of what you want to achieve, what you tried, what you saw & what you expected to see. Please take a look at How to Ask & tour for more information on how stack exchange works and work through the Robotics question checklist to edit your question to make it clearer.
– Tully
Commented Mar 20 at 6:05