0
$\begingroup$

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
#############################################################

'''

$\endgroup$
1
  • $\begingroup$ 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. $\endgroup$
    – Tully
    Commented Mar 20 at 6:05

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.