by

Angle estimation using Kalman filter

Keywords: Sensor fushion, angle estimation, gyrometer, accelerometer, kalman filter

In order to estimate angle using gyrometer- and accelerometer data I decided to implement a Kalman filter. I won’t dig much into the theory as plenty other sources on the interwebs are more qualified for that.

Shown is the completed result of the Kalman filter in action during a 20 second period, where it can be seen that the resulting curve (blue) follows the gyro (green) short term but is being corrected by the accelerometer (red) long term.

kalman

I decided to implement the system matrices as a struct so it can be easily modified for other sensors and purposes, though only in two dimensions meaning only two different sensor inputs can be used at the time.

typedef struct{
    // Inputs
    float u;        // Control input (for angle estimation use gyro data)
    float z[2];     // Measurement input (for angle estimation use acc data)
    float dt;       // Timestamp
    // Output matrix
    float x[2];     // Output
    // Initialize variance for each sensor
    float Q[2];     // Process (control) variance (for u)
    float R[2];     // Measurement variance (for z)
    // Initialize models
    float H[2][2];  // Observation model
    float F[2][2];  // State transition model
    float B[2];     // Control input model
    float P[2][2];  // Covariance matrix
    // Don't initialize
    float y[2];
    float S[2][2];
    float K[2][2];
} FilterKalman_t;

In order to initialize it, the state transition model F as well as the control model B has to be found. They can be found by looking at the units of our inputs and desired outputs, where our desired output vector is angle and gyro bias:

    \[ x_{k} = \begin{bmatrix} \theta \\ \dot{\theta}_{b} \end{bmatrix}_{k} \]

The first element in the input vector z is acceleration which directly can be translated into an angle, hence is already the correct unit. Our control input u is the gyrometer (angular velocity) which can be translated into an angle by calculating the temporal integral. The full expression of our first output parameter would be:

angle = initial angle + (anglular velocity * dt)

You might recognize the formula from high school science class.

Since an angle can be directly extracted from acceleration, and our angular velocity (gyro) is drifting, we change the expression to:

angle = acceleration + ((gyro - gyro_bias) * dt)

The state of the system

    \[ x_{k} = \boldsymbol{F}x_{k-1} + \boldsymbol{B}u_{k} \]

hence gives us the expression:

    \[ \begin{bmatrix} \theta \\ \dot{\theta}_{b} \end{bmatrix}_{k} = \begin{bmatrix} 1 & -dt \\ 0 & 1 \end{bmatrix} \begin{bmatrix} \theta \\ \dot{\theta} \end{bmatrix}_{k-1} + \begin{bmatrix} dt \\ 0 \end{bmatrix} u_{k} \]

The full initialization will be:

FilterKalman_t angleFilterKalman = {
    .F[0][0] = 1,
    .F[0][1] = -dt,
    .F[1][0] = 0,
    .F[1][1] = 1,
    .B[0] = dt,
    .B[1] = 0,
    .Q[0] = 0.001,  // Variance for gyro
    .Q[1] = 0.003,  // Variance for gyro bias
    .R[0] = 0.03,   // Variance for accelerometer
    .R[1] = 0.03,   // Variance for accelerometer
    .H[0][0] = 1,
    .H[0][1] = 0,
    .H[1][0] = 0,
    .H[1][1] = 0,
 
    .P[0][0] = 0,
    .P[0][1] = 0,
    .P[1][0] = 0,
    .P[1][1] = 0
};

The filter function itself is implemented together with comments for it to be quite straight-forward.

void Filter_Kalman(FilterKalman_t *k)
{
    // x = F*x + B*u
    float x_temp[2];
    x_temp[0] = k->F[0][0] * k->x[0] + k->F[0][1] * k->x[1] + k->B[0]*k->u; // Position
    x_temp[1] = k->F[1][0] * k->x[0] + k->F[1][1] * k->x[1] + k->B[1]*k->u; // Velocity
    k->x[0] = x_temp[0];
    k->x[1] = x_temp[1];
    //rate = u - x[1];
 
    // P = F * P * Ft + Q
    float P_temp[2][2];
    P_temp[0][0] = k->F[0][0] * (k->F[0][0]*k->P[0][0]+k->F[0][1]*k->P[1][0]) + k->F[0][1] * (k->F[0][0]*k->P[0][1]+k->F[0][1]*k->P[1][1]) + k->Q[0]*k->dt;
    P_temp[0][1] = k->F[1][0] * (k->F[0][0]*k->P[0][0]+k->F[0][1]*k->P[1][0]) + k->F[1][1] * (k->F[0][0]*k->P[0][1]+k->F[0][1]*k->P[1][1]);
    P_temp[1][0] = k->F[0][0] * (k->F[1][0]*k->P[0][0]+k->F[1][1]*k->P[1][0]) + k->F[0][1] * (k->F[1][0]*k->P[0][1]+k->F[1][1]*k->P[1][1]);
    P_temp[1][1] = k->F[1][0] * (k->F[1][0]*k->P[0][0]+k->F[1][1]*k->P[1][0]) + k->F[1][1] * (k->F[1][0]*k->P[0][1]+k->F[1][1]*k->P[1][1]) + k->Q[1]*k->dt;
 
    k->P[0][0] = P_temp[0][0];
    k->P[0][1] = P_temp[0][1];
    k->P[1][0] = P_temp[1][0];
    k->P[1][1] = P_temp[1][1];
 
    // S = H*P * Ht + R
    k->S[0][0] = k->H[0][0] * k->H[0][0] * k->P[0][0] + k->R[0];
    k->S[0][1] = k->H[0][1] * k->H[1][0] * k->P[0][1];
    k->S[1][0] = k->H[0][1] * k->H[1][0] * k->P[1][0];
    k->S[1][1] = k->H[1][1] * k->H[1][1] * k->P[1][1] + k->R[1];
 
    // K = P * Ht * S^-1
    float S_inv = k->S[0][0]*k->S[1][1] - k->S[0][1]*k->S[1][0];
 
    k->K[0][0] = ((k->H[0][0]*k->P[0][0] + k->H[0][1]*k->P[0][1])*k->S[1][1] - (k->H[1][0]*k->P[0][0] + k->H[1][1]*k->P[0][1])*k->S[1][0]) / S_inv;
    k->K[0][1] = ((k->H[1][0]*k->P[0][0] + k->H[1][1]*k->P[0][1])*k->S[0][0] - (k->H[0][0]*k->P[0][0] + k->H[0][1]*k->P[0][1])*k->S[0][1]) / S_inv;
    k->K[1][0] = ((k->H[0][0]*k->P[1][0] + k->H[0][1]*k->P[1][1])*k->S[1][1] - (k->H[1][0]*k->P[1][0] + k->H[1][1]*k->P[1][1])*k->S[1][0]) / S_inv;
    k->K[1][1] = ((k->H[1][0]*k->P[1][0] + k->H[1][1]*k->P[1][1])*k->S[0][0] - (k->H[0][0]*k->P[1][0] + k->H[0][1]*k->P[1][1])*k->S[0][1]) / S_inv;
 
    // y = z - (H*x)
    k->y[0] = k->z[0] - (k->H[0][0]*k->x[0] + k->H[0][1]*k->x[1]);
    k->y[1] = k->z[1] - (k->H[1][0]*k->x[0] + k->H[1][1]*k->x[1]);
 
    // x = x + (K*y)
    k->x[0] = k->x[0] + k->K[0][0]*k->y[0] + k->K[0][1]*k->y[1];
    k->x[1] = k->x[1] + k->K[1][0]*k->y[0] + k->K[1][1]*k->y[1];
 
    // P = (I - K*H)*P
    P_temp[0][0] = k->P[0][0]*(1 - k->H[0][0]*k->K[0][0] - k->H[1][0]*k->K[0][1]);
    P_temp[0][1] = k->P[0][1]*(0 - k->H[0][1]*k->K[0][0] - k->H[1][1]*k->K[0][1]);
    P_temp[1][0] = k->P[1][0]*(0 - k->H[0][0]*k->K[1][0] - k->H[1][0]*k->K[1][1]);
    P_temp[1][1] = k->P[1][1]*(1 - k->H[0][1]*k->K[1][0] - k->H[1][1]*k->K[1][1]);
 
    k->P[0][0] = P_temp[0][0];
    k->P[0][1] = P_temp[0][1];
    k->P[1][0] = P_temp[1][0];
    k->P[1][1] = P_temp[1][1];
}

The full source code can be found here, but has probably been improved since this post was written.

Write a Comment

Comment