test
kalman.c@1:270647f1e086, 2021-03-28 (annotated)
- Committer:
- fitzpatrick
- Date:
- Sun Mar 28 20:19:54 2021 +0000
- Revision:
- 1:270647f1e086
- Parent:
- 0:8a460b0d6f09
To Debug Ethernet Includes
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 * |
fitzpatrick | 1:270647f1e086 | 12 | * Parameters: struct filter, R_angle, Q_gyro, Q_angle * |
cdonate | 0:8a460b0d6f09 | 13 | * Description: Initialize the kalman Filter parameters. * |
cdonate | 0:8a460b0d6f09 | 14 | ********************************************************************/ |
cdonate | 0:8a460b0d6f09 | 15 | |
fitzpatrick | 1:270647f1e086 | 16 | void kalman_init(kalman *filter, double R_angle, double Q_gyro, double Q_angle) // Initialization |
cdonate | 0:8a460b0d6f09 | 17 | { |
fitzpatrick | 1:270647f1e086 | 18 | // Initialize the three states, the angle, gyro bias and angular rate |
cdonate | 0:8a460b0d6f09 | 19 | filter->x_bias = 0.0; |
cdonate | 0:8a460b0d6f09 | 20 | filter->x_rate = 0.0; |
cdonate | 0:8a460b0d6f09 | 21 | filter->x_angle = 0.0; |
cdonate | 0:8a460b0d6f09 | 22 | |
fitzpatrick | 1:270647f1e086 | 23 | // Initialize measurement noise covariance, a 1x1 matrix representing expected accelerometer jitter |
fitzpatrick | 1:270647f1e086 | 24 | // R_angel is set to 0.59 |
cdonate | 0:8a460b0d6f09 | 25 | filter->R_angle = R_angle; |
cdonate | 0:8a460b0d6f09 | 26 | |
fitzpatrick | 1:270647f1e086 | 27 | // Initialize process noise covariance matrix, trust of acceleromter vs trust of gyro |
fitzpatrick | 1:270647f1e086 | 28 | // Q_gyro set to 0.002 and Q_angle set to 0.001. |
cdonate | 0:8a460b0d6f09 | 29 | |
cdonate | 0:8a460b0d6f09 | 30 | filter->Q_gyro = Q_gyro; |
cdonate | 0:8a460b0d6f09 | 31 | filter->Q_angle = Q_angle; |
cdonate | 0:8a460b0d6f09 | 32 | |
cdonate | 0:8a460b0d6f09 | 33 | // Initialize covariance of estimate state. This is updated |
cdonate | 0:8a460b0d6f09 | 34 | // at every time step to determine how well the sensors are |
fitzpatrick | 1:270647f1e086 | 35 | // tracking the actual state. Initially an Identity matrix |
cdonate | 0:8a460b0d6f09 | 36 | |
cdonate | 0:8a460b0d6f09 | 37 | filter->P_00 = 1.0; |
cdonate | 0:8a460b0d6f09 | 38 | filter->P_01 = 0.0; |
cdonate | 0:8a460b0d6f09 | 39 | filter->P_10 = 0.0; |
cdonate | 0:8a460b0d6f09 | 40 | filter->P_11 = 1.0; |
cdonate | 0:8a460b0d6f09 | 41 | } |
cdonate | 0:8a460b0d6f09 | 42 | |
cdonate | 0:8a460b0d6f09 | 43 | /******************************************************************** |
fitzpatrick | 1:270647f1e086 | 44 | * Function Name: kalman_predict * |
cdonate | 0:8a460b0d6f09 | 45 | * Return Value: none * |
fitzpatrick | 1:270647f1e086 | 46 | * Parameters: struct filter, measured gyro value * |
fitzpatrick | 1:270647f1e086 | 47 | * Description: Called at dt, updates current rate and angle * |
cdonate | 0:8a460b0d6f09 | 48 | ********************************************************************/ |
cdonate | 0:8a460b0d6f09 | 49 | void kalman_predict(kalman *filter, double dot_angle, double dt) |
cdonate | 0:8a460b0d6f09 | 50 | { |
fitzpatrick | 1:270647f1e086 | 51 | // so not on the stack. |
cdonate | 0:8a460b0d6f09 | 52 | static double gyro_rate_unbiased; |
cdonate | 0:8a460b0d6f09 | 53 | static double Pdot_00; |
cdonate | 0:8a460b0d6f09 | 54 | static double Pdot_01; |
cdonate | 0:8a460b0d6f09 | 55 | static double Pdot_10; |
cdonate | 0:8a460b0d6f09 | 56 | static double Pdot_11; |
cdonate | 0:8a460b0d6f09 | 57 | |
fitzpatrick | 1:270647f1e086 | 58 | // dot_angle is the gyro value. |
fitzpatrick | 1:270647f1e086 | 59 | gyro_rate_unbiased = dot_angle - filter->x_bias; |
cdonate | 0:8a460b0d6f09 | 60 | |
cdonate | 0:8a460b0d6f09 | 61 | // Store our unbiased gyro estimate. |
cdonate | 0:8a460b0d6f09 | 62 | filter->x_rate= gyro_rate_unbiased; |
cdonate | 0:8a460b0d6f09 | 63 | |
fitzpatrick | 1:270647f1e086 | 64 | // x_angle = x_angle + (measured gyro - x_bias)*dt from X = AX(k-1) + Bu(k-1) |
cdonate | 0:8a460b0d6f09 | 65 | filter->x_angle= filter->x_angle + (dot_angle - filter->x_bias)*dt; |
cdonate | 0:8a460b0d6f09 | 66 | |
cdonate | 0:8a460b0d6f09 | 67 | // Compute the derivative of the covariance matrix |
cdonate | 0:8a460b0d6f09 | 68 | // Pdot = A*P + P*A' + Q |
cdonate | 0:8a460b0d6f09 | 69 | Pdot_00 = filter->Q_angle - filter->P_01 - filter->P_10; |
cdonate | 0:8a460b0d6f09 | 70 | Pdot_01 = -filter->P_11; |
cdonate | 0:8a460b0d6f09 | 71 | Pdot_10 = -filter->P_11; |
cdonate | 0:8a460b0d6f09 | 72 | Pdot_11 = filter->Q_gyro; |
cdonate | 0:8a460b0d6f09 | 73 | |
cdonate | 0:8a460b0d6f09 | 74 | // Update the covariance matrix. |
cdonate | 0:8a460b0d6f09 | 75 | filter->P_00 += Pdot_00 * dt; |
cdonate | 0:8a460b0d6f09 | 76 | filter->P_01 += Pdot_01 * dt; |
cdonate | 0:8a460b0d6f09 | 77 | filter->P_10 += Pdot_10 * dt; |
cdonate | 0:8a460b0d6f09 | 78 | filter->P_11 += Pdot_11 * dt; |
cdonate | 0:8a460b0d6f09 | 79 | } |
cdonate | 0:8a460b0d6f09 | 80 | /********************************************************************* |
cdonate | 0:8a460b0d6f09 | 81 | * Function Name: kalman_update * |
cdonate | 0:8a460b0d6f09 | 82 | * Return Value: none * |
cdonate | 0:8a460b0d6f09 | 83 | * Parameters: struct filter, measured angle value * |
fitzpatrick | 1:270647f1e086 | 84 | * Description: Called with new accelerometer angle * |
fitzpatrick | 1:270647f1e086 | 85 | * Updates estimated angle * |
cdonate | 0:8a460b0d6f09 | 86 | *********************************************************************/ |
cdonate | 0:8a460b0d6f09 | 87 | void kalman_update(kalman *filter, double angle_measured) |
cdonate | 0:8a460b0d6f09 | 88 | { |
fitzpatrick | 1:270647f1e086 | 89 | // Static to keep off the stack. |
cdonate | 0:8a460b0d6f09 | 90 | static double y; |
cdonate | 0:8a460b0d6f09 | 91 | static double S; |
cdonate | 0:8a460b0d6f09 | 92 | static double K_0; |
cdonate | 0:8a460b0d6f09 | 93 | static double K_1; |
cdonate | 0:8a460b0d6f09 | 94 | |
cdonate | 0:8a460b0d6f09 | 95 | // Compute the error in the estimate. |
cdonate | 0:8a460b0d6f09 | 96 | // Innovation or Measurement Residual |
cdonate | 0:8a460b0d6f09 | 97 | // y = z - Hx |
cdonate | 0:8a460b0d6f09 | 98 | y= angle_measured - filter->x_angle; |
cdonate | 0:8a460b0d6f09 | 99 | |
cdonate | 0:8a460b0d6f09 | 100 | // Compute the error estimate. |
fitzpatrick | 1:270647f1e086 | 101 | // S = H P H' + R |
cdonate | 0:8a460b0d6f09 | 102 | S = filter->R_angle + filter->P_00; |
cdonate | 0:8a460b0d6f09 | 103 | |
cdonate | 0:8a460b0d6f09 | 104 | // Compute the kalman filter gains. |
fitzpatrick | 1:270647f1e086 | 105 | // K = P H' inv(S) = P H'/(H P H' + R) |
cdonate | 0:8a460b0d6f09 | 106 | K_0 = filter->P_00 / S; |
cdonate | 0:8a460b0d6f09 | 107 | K_1 = filter->P_10 / S; |
cdonate | 0:8a460b0d6f09 | 108 | |
cdonate | 0:8a460b0d6f09 | 109 | // Update covariance matrix. |
fitzpatrick | 1:270647f1e086 | 110 | // P = P - K H P = (I - KH)P |
cdonate | 0:8a460b0d6f09 | 111 | filter->P_00 -= K_0 * filter->P_00; |
cdonate | 0:8a460b0d6f09 | 112 | filter->P_01 -= K_0 * filter->P_01; |
cdonate | 0:8a460b0d6f09 | 113 | filter->P_10 -= K_1 * filter->P_00; |
cdonate | 0:8a460b0d6f09 | 114 | filter->P_11 -= K_1 * filter->P_01; |
cdonate | 0:8a460b0d6f09 | 115 | |
cdonate | 0:8a460b0d6f09 | 116 | // Update the state (new)estimates (Correct the prediction of the state). |
cdonate | 0:8a460b0d6f09 | 117 | // Also adjust the bias on the gyro at every iteration. |
fitzpatrick | 1:270647f1e086 | 118 | // x = x + K * y = x + K(z - Hx ) |
cdonate | 0:8a460b0d6f09 | 119 | filter->x_angle = filter->x_angle + K_0 * y; |
cdonate | 0:8a460b0d6f09 | 120 | filter->x_bias = filter->x_bias + K_1 * y; |
cdonate | 0:8a460b0d6f09 | 121 | |
cdonate | 0:8a460b0d6f09 | 122 | } |