John Fitzpatrick / kalman

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?

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 *
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 }