Error state Kalman Filter from MATLAB to Python

Question

I am trying to reproduce the algorithm explained here in Python but I am facing some problems with a strange growth of some parameters. The following is my attempt. Observe that get_ang() and get_acc() return angular velocity and acceleration along [x,y,z]-axis in degrees/s (but I convert this data in radians/s) and m/s^2 respectively):

import numpy as np
import quaternion
from utils import get_ang, get_acc

#utils
Z=np.zeros([3,3])
I=np.eye(3)
EARTH_GRAVITY_MS2 = -9.80665

#sample parameters
N=1        #DecimationFactor
fs=10      #SampleRate

#noise parameters
beta=3.0462e-13      #GyroscopeDriftNoise
eta=9.1385e-5       #GyroscopeNoise
kappa=N/fs  #DecimationFactor/SampleRate
lamb=0.00019247      #AccelerometerNoise
nu=0.5        #LinearAccelerationDecayFactor
csi=0.0096236       #LinearAccelerationNoise

#other parameters initialization
lin_acc_prior=np.zeros(3)
gyro_offset=np.zeros([1,3])
Q=np.diag([0.000006092348396, 0.000006092348396, 0.000006092348396, 0.000076154354947,0.000076154354947, 0.000076154354947,0.009623610000000, 0.009623610000000, 0.009623610000000])
R=(lamb+csi+(kappa**2)*(beta+eta))*I
P=Q
q=quaternion.quaternion(1,0,0,0)                     


while(1):

    """----------------------------------------------------------Model----------------------------------------------------------"""

    """Predict orientation (q-)"""
    gyro_readings=np.array(np.radians([get_ang()])) #rad/s

    for i in range(N-1):
        gyro_readings=np.append(gyro_readings,np.radians([get_ang()]),axis=0)

    delta_phi=(gyro_readings-gyro_offset)/fs    #rad/s  
    delta_q=quaternion.from_rotation_vector(delta_phi)
    q=q*np.prod(delta_q)

    """Estimate gravity from orientation (g)"""
    r_prior=quat2rotm(q) 
    g=r_prior[:,2:3].transpose()*(-EARTH_GRAVITY_MS2)   #m/s^2

    """Estimate gravity from acceleration (g_acc)"""
    accel_readings=get_acc() #m/s^2
    g_acc=accel_readings-lin_acc_prior #m/s^2


    """----------------------------------------------------------Error Model----------------------------------------------------------"""

    "Error Model (z)"
    z=g-g_acc #m/s^2

    """----------------------------------------------------------Kalman Equations----------------------------------------------------------"""

    """Observation model (H)"""
    gx=g[0,0]
    gy=g[0,1]
    gz=g[0,2]
    g_cross=np.array([[0, gz, -gy],[-gz, 0, gx],[gy, -gx, 0]])
    H=np.block([g_cross, -kappa*g_cross, I])

    """Innovation covariance (S)""" 
    S=R+np.dot(H,np.dot(P,H.transpose()))

    """Kalman gain (K)"""
    K=np.dot(P,np.dot(H.transpose(),np.linalg.inv(S)))

    """Update error estimate covariance (P+)"""
    P=P-np.dot(K,np.dot(H,P))

    """Predict error estimate covariance (P-)"""
    D1=np.diag(np.diag(P[0:3,0:3]))   #first diagonal block P
    D2=np.diag(np.diag(P[3:6,3:6]))   #second diagonal block P
    D3=np.diag(np.diag(P[6:9,6:9]))   #third diagonal block P

    Q11=D1+kappa**2*D2+(beta+eta)*I
    Q12=(D2+beta*I)
    Q12[0,0]*=-kappa
    Q22=D2+beta*I
    Q33=nu**2*D3+csi*I

    Q=np.block([[Q11,Q12,Z],[Q12,Q22,Z],[Z,Z,Q33]])
    P=Q

    """Update posterior error (x)"""
    x=np.dot(K,z.transpose())

    """----------------------------------------------------------Correct----------------------------------------------------------"""

    """Estimate orientation (q+)"""
    theta=x[:3].transpose() #rad
    q=q*quaternion.from_rotation_vector(-theta)[0]




    """Estimate linear acceleration (lin_acc_prior)"""
    b=x[3:6].transpose() #rad/s
    lin_acc_prior = lin_acc_prior*nu-b

    """Estimate gyro offset (gyro_offset)"""
    a=x[6:].transpose()
    gyro_offset=gyro_offset-a

    """----------------------------------------------------------Compute Angular Velocity----------------------------------------------------------"""

    """Angular velocity (angular_velocity)"""
    angular_velocity=np.sum(gyro_readings,axis=0)/N-gyro_offset

With my IMU remaining stationary (get_ang returning values around [0,0,0] and get_acc returning values around [0,0,-9.8]) I observe anomalous growth of gyro_offset (probably due to not small value of a) resulting in a wrong computation of delta_phi, delta_q, q and so in a wrong estimation of g and z.

I checked my code a lot of times but I didn't found any mistake. I thought that I could misinterpret the instruction in the link above, maybe making confusion with measure units (degrees, radians, m/s^2, g), but even trying with different combinations I obtain a similar behaviour.

Could you please help me to find what I am missing?

P.S. It is possible to reproduce my setting putting at each step:

gyro_readings=np.random.normal(0,1,[1,3])/50 
accel_readings=np.array([0,0,-9.8])+np.random.normal(0,1,[3])/50

---

Answer 1

In the link you provided under kalman equations, transpose of S is inverted to calculate kalman gain.
It seems like you didn't take the transpose of S before inverting it. in the line :

K=np.dot(P,np.dot(H.transpose(),np.linalg.inv(S)))

it should be

K=np.dot(P,np.dot(H.transpose(),np.linalg.inv(S.transpose())))

Answer 2

I see following problems in your code:

• when calculating g from the orientation matrix you multiplied it by the earth gravity. As result your observation error and hence the innovation are measured in m/s2. According to the documentation the filter needs the error in units g. So I would rather divide the g_acc by the earth gravity.

• when accessing the state vector x you used the elements 4:6 for the linear acceleration estimation, but those elements belong to the gyro offset. The same thing is with the elements 7:9 which should be used for the acceleration, not for the gyro offset.

• when generating the test signals you used some parameters for the normal distribution to simulate the noise. I would use exactly the same noise parameters you used in the filter implementation, otherwise those two noise levels do not correspond to each other and the filter could not perform optimally.

• the formula for Q given on the matlab page does not correspond to the original formula in the documentation. Compare equations 10.1.23 and 10.1.24. They involve P elements [0,2:3,5] and [3,5:3,5] respectively. In your case it means the submatrix Q12 is not correct.

Unfortunatelly I could not run your python code to check if it works better with the suggestions above. But my matlab code showed a better performance with them.

Could you try it and post some plots?