test
kalman.c@0:8a460b0d6f09, 2012-08-15 (annotated)
- Committer:
- cdonate
- Date:
- Wed Aug 15 22:17:07 2012 +0000
- Revision:
- 0:8a460b0d6f09
- Child:
- 1:270647f1e086
Kalman Filter fusing input from the Accelerometer and Gyroscope to output PITCH and ROLL.
Who changed what in which revision?
User | Revision | Line number | New 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 | } |