Library for a Kalman filter that receive inputs from accelerometer and gyroscope and outputs PITCH and ROLL

Dependents:   KalmanFilter Freescale_Multi-Sensor_Quadcopter FRDM_EMG_Galileo_Hand Galileo_HoverBoardRider ... more

Committer:
cdonate
Date:
Wed Aug 15 22:17:07 2012 +0000
Revision:
0:8a460b0d6f09
Kalman Filter fusing input from the Accelerometer and Gyroscope to output PITCH and ROLL.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cdonate 0:8a460b0d6f09 1 #include "kalman.h"
cdonate 0:8a460b0d6f09 2
cdonate 0:8a460b0d6f09 3 /*********************************************************************
cdonate 0:8a460b0d6f09 4 The implememted kalman filter estimates the angle that will be used
cdonate 0:8a460b0d6f09 5 on the PID controller alogrithm. The filter consists of two states
cdonate 0:8a460b0d6f09 6 [angle, gyro_bais].
cdonate 0:8a460b0d6f09 7 *********************************************************************/
cdonate 0:8a460b0d6f09 8
cdonate 0:8a460b0d6f09 9 /********************************************************************
cdonate 0:8a460b0d6f09 10 * Function Name: kalman_init *
cdonate 0:8a460b0d6f09 11 * Return Value: none *
cdonate 0:8a460b0d6f09 12 * Parameters: struct filter, dt, R_angle, Q_gyro, Q_angle *
cdonate 0:8a460b0d6f09 13 * Description: Initialize the kalman Filter parameters. *
cdonate 0:8a460b0d6f09 14 ********************************************************************/
cdonate 0:8a460b0d6f09 15
cdonate 0:8a460b0d6f09 16 void kalman_init(kalman *filter, double R_angle, double Q_gyro, double Q_angle) // double dt,
cdonate 0:8a460b0d6f09 17 {
cdonate 0:8a460b0d6f09 18 // Initialize the two states, the angle and the gyro bias. As a
cdonate 0:8a460b0d6f09 19 // byproduct of computing the angle, we also have an unbiased
cdonate 0:8a460b0d6f09 20 // angular rate available.
cdonate 0:8a460b0d6f09 21 filter->x_bias = 0.0;
cdonate 0:8a460b0d6f09 22 filter->x_rate = 0.0;
cdonate 0:8a460b0d6f09 23 filter->x_angle = 0.0;
cdonate 0:8a460b0d6f09 24
cdonate 0:8a460b0d6f09 25
cdonate 0:8a460b0d6f09 26 // Initialize the delta in seconds between gyro samples.
cdonate 0:8a460b0d6f09 27 //filter->dt = dt;
cdonate 0:8a460b0d6f09 28
cdonate 0:8a460b0d6f09 29 // Initialize the measurement noise covariance matrix values.
cdonate 0:8a460b0d6f09 30 // In this case, R is a 1x1 matrix tha represents expected
cdonate 0:8a460b0d6f09 31 // jitter from the accelerometer.
cdonate 0:8a460b0d6f09 32 filter->R_angle = R_angle;
cdonate 0:8a460b0d6f09 33
cdonate 0:8a460b0d6f09 34 // Initialize the process noise covariance matrix values.
cdonate 0:8a460b0d6f09 35 // In this case, Q indicates how much we trust the acceleromter
cdonate 0:8a460b0d6f09 36 // relative to the gyros.
cdonate 0:8a460b0d6f09 37 // Q_gyro set to 0.003 and Q_angle set to 0.001.
cdonate 0:8a460b0d6f09 38
cdonate 0:8a460b0d6f09 39 filter->Q_gyro = Q_gyro;
cdonate 0:8a460b0d6f09 40 filter->Q_angle = Q_angle;
cdonate 0:8a460b0d6f09 41
cdonate 0:8a460b0d6f09 42 // Initialize covariance of estimate state. This is updated
cdonate 0:8a460b0d6f09 43 // at every time step to determine how well the sensors are
cdonate 0:8a460b0d6f09 44 // tracking the actual state.
cdonate 0:8a460b0d6f09 45
cdonate 0:8a460b0d6f09 46 filter->P_00 = 1.0;
cdonate 0:8a460b0d6f09 47 filter->P_01 = 0.0;
cdonate 0:8a460b0d6f09 48 filter->P_10 = 0.0;
cdonate 0:8a460b0d6f09 49 filter->P_11 = 1.0;
cdonate 0:8a460b0d6f09 50 }
cdonate 0:8a460b0d6f09 51
cdonate 0:8a460b0d6f09 52 /********************************************************************
cdonate 0:8a460b0d6f09 53 * Function Name: kalman_predict *
cdonate 0:8a460b0d6f09 54 * Return Value: none *
cdonate 0:8a460b0d6f09 55 * Parameters: struct filter, measured gyroscope value *
cdonate 0:8a460b0d6f09 56 * Description: Called every dt(Timer 1 overflow with a biased *
cdonate 0:8a460b0d6f09 57 * gyro. Also updates the current rate and angle *
cdonate 0:8a460b0d6f09 58 * estimate). *
cdonate 0:8a460b0d6f09 59 ********************************************************************/
cdonate 0:8a460b0d6f09 60 void kalman_predict(kalman *filter, double dot_angle, double dt)
cdonate 0:8a460b0d6f09 61 {
cdonate 0:8a460b0d6f09 62 // Static so these are kept off the stack.
cdonate 0:8a460b0d6f09 63 static double gyro_rate_unbiased;
cdonate 0:8a460b0d6f09 64 static double Pdot_00;
cdonate 0:8a460b0d6f09 65 static double Pdot_01;
cdonate 0:8a460b0d6f09 66 static double Pdot_10;
cdonate 0:8a460b0d6f09 67 static double Pdot_11;
cdonate 0:8a460b0d6f09 68
cdonate 0:8a460b0d6f09 69 // Unbias our gyro.
cdonate 0:8a460b0d6f09 70 gyro_rate_unbiased= dot_angle - filter->x_bias;
cdonate 0:8a460b0d6f09 71
cdonate 0:8a460b0d6f09 72 // Store our unbiased gyro estimate.
cdonate 0:8a460b0d6f09 73 filter->x_rate= gyro_rate_unbiased;
cdonate 0:8a460b0d6f09 74
cdonate 0:8a460b0d6f09 75 // Update the angle estimate.
cdonate 0:8a460b0d6f09 76 filter->x_angle= filter->x_angle + (dot_angle - filter->x_bias)*dt;
cdonate 0:8a460b0d6f09 77
cdonate 0:8a460b0d6f09 78 // Compute the derivative of the covariance matrix
cdonate 0:8a460b0d6f09 79 // Pdot = A*P + P*A' + Q
cdonate 0:8a460b0d6f09 80 Pdot_00 = filter->Q_angle - filter->P_01 - filter->P_10;
cdonate 0:8a460b0d6f09 81 Pdot_01 = -filter->P_11;
cdonate 0:8a460b0d6f09 82 Pdot_10 = -filter->P_11;
cdonate 0:8a460b0d6f09 83 Pdot_11 = filter->Q_gyro;
cdonate 0:8a460b0d6f09 84
cdonate 0:8a460b0d6f09 85 // Update the covariance matrix.
cdonate 0:8a460b0d6f09 86 filter->P_00 += Pdot_00 * dt;
cdonate 0:8a460b0d6f09 87 filter->P_01 += Pdot_01 * dt;
cdonate 0:8a460b0d6f09 88 filter->P_10 += Pdot_10 * dt;
cdonate 0:8a460b0d6f09 89 filter->P_11 += Pdot_11 * dt;
cdonate 0:8a460b0d6f09 90 }
cdonate 0:8a460b0d6f09 91 /*********************************************************************
cdonate 0:8a460b0d6f09 92 * Function Name: kalman_update *
cdonate 0:8a460b0d6f09 93 * Return Value: none *
cdonate 0:8a460b0d6f09 94 * Parameters: struct filter, measured angle value *
cdonate 0:8a460b0d6f09 95 * Description: Called when a new accelerometer angle *
cdonate 0:8a460b0d6f09 96 * measurement is available. Updates the estimated *
cdonate 0:8a460b0d6f09 97 * angle that will be used. *
cdonate 0:8a460b0d6f09 98 *********************************************************************/
cdonate 0:8a460b0d6f09 99 void kalman_update(kalman *filter, double angle_measured)
cdonate 0:8a460b0d6f09 100 {
cdonate 0:8a460b0d6f09 101 // Static so these are kept off the stack.
cdonate 0:8a460b0d6f09 102 static double y;
cdonate 0:8a460b0d6f09 103 static double S;
cdonate 0:8a460b0d6f09 104 static double K_0;
cdonate 0:8a460b0d6f09 105 static double K_1;
cdonate 0:8a460b0d6f09 106
cdonate 0:8a460b0d6f09 107 // Compute the error in the estimate.
cdonate 0:8a460b0d6f09 108 // Innovation or Measurement Residual
cdonate 0:8a460b0d6f09 109 // y = z - Hx
cdonate 0:8a460b0d6f09 110 y= angle_measured - filter->x_angle;
cdonate 0:8a460b0d6f09 111
cdonate 0:8a460b0d6f09 112 // Compute the error estimate.
cdonate 0:8a460b0d6f09 113 // S = C P C' + R
cdonate 0:8a460b0d6f09 114 S = filter->R_angle + filter->P_00;
cdonate 0:8a460b0d6f09 115
cdonate 0:8a460b0d6f09 116 // Compute the kalman filter gains.
cdonate 0:8a460b0d6f09 117 // K = P C' inv(S)
cdonate 0:8a460b0d6f09 118 K_0 = filter->P_00 / S;
cdonate 0:8a460b0d6f09 119 K_1 = filter->P_10 / S;
cdonate 0:8a460b0d6f09 120
cdonate 0:8a460b0d6f09 121 // Update covariance matrix.
cdonate 0:8a460b0d6f09 122 // P = P - K C P
cdonate 0:8a460b0d6f09 123 filter->P_00 -= K_0 * filter->P_00;
cdonate 0:8a460b0d6f09 124 filter->P_01 -= K_0 * filter->P_01;
cdonate 0:8a460b0d6f09 125 filter->P_10 -= K_1 * filter->P_00;
cdonate 0:8a460b0d6f09 126 filter->P_11 -= K_1 * filter->P_01;
cdonate 0:8a460b0d6f09 127
cdonate 0:8a460b0d6f09 128 // Update the state (new)estimates (Correct the prediction of the state).
cdonate 0:8a460b0d6f09 129 // Also adjust the bias on the gyro at every iteration.
cdonate 0:8a460b0d6f09 130 // x = x + K * y
cdonate 0:8a460b0d6f09 131 filter->x_angle = filter->x_angle + K_0 * y;
cdonate 0:8a460b0d6f09 132 filter->x_bias = filter->x_bias + K_1 * y;
cdonate 0:8a460b0d6f09 133
cdonate 0:8a460b0d6f09 134 }