Ahmad Kobeissi / X_NUCLEO_COMMON

Computes Euler angles

Fork of X_NUCLEO_COMMON by ST

Kalman.h@23:fb5f8e018461, 2017-11-13 (annotated)

Committer:
ahmad47
Date:
Mon Nov 13 10:45:20 2017 +0000
Revision:
23:fb5f8e018461
Computes Pitch and roll according to kalman and heading according to custom algorithm

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ahmad47 23:fb5f8e018461 1 #ifndef _Kalman_h
ahmad47 23:fb5f8e018461 2 #define _Kalman_h
ahmad47 23:fb5f8e018461 3
ahmad47 23:fb5f8e018461 4 class Kalman {
ahmad47 23:fb5f8e018461 5 public:
ahmad47 23:fb5f8e018461 6 Kalman() {
ahmad47 23:fb5f8e018461 7 /* We will set the variables like so, these can also be tuned by the user */
ahmad47 23:fb5f8e018461 8 Q_angle = 0.001;
ahmad47 23:fb5f8e018461 9 Q_bias = 0.003;
ahmad47 23:fb5f8e018461 10 R_measure = 0.03;
ahmad47 23:fb5f8e018461 11
ahmad47 23:fb5f8e018461 12 angle = 0; // Reset the angle
ahmad47 23:fb5f8e018461 13 bias = 0; // Reset bias
ahmad47 23:fb5f8e018461 14
ahmad47 23:fb5f8e018461 15 P[0][0] = 0; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so
ahmad47 23:fb5f8e018461 16 P[0][1] = 0;
ahmad47 23:fb5f8e018461 17 P[1][0] = 0;
ahmad47 23:fb5f8e018461 18 P[1][1] = 0;
ahmad47 23:fb5f8e018461 19 };
ahmad47 23:fb5f8e018461 20 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
ahmad47 23:fb5f8e018461 21 double getAngle(double newAngle, double newRate, double dt) {
ahmad47 23:fb5f8e018461 22
ahmad47 23:fb5f8e018461 23 // Discrete Kalman filter time update equations - Time Update ("Predict")
ahmad47 23:fb5f8e018461 24 // Update xhat - Project the state ahead
ahmad47 23:fb5f8e018461 25 /* Step 1 */
ahmad47 23:fb5f8e018461 26 rate = newRate - bias;
ahmad47 23:fb5f8e018461 27 angle += dt * rate;
ahmad47 23:fb5f8e018461 28
ahmad47 23:fb5f8e018461 29 // Update estimation error covariance - Project the error covariance ahead
ahmad47 23:fb5f8e018461 30 /* Step 2 */
ahmad47 23:fb5f8e018461 31 P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
ahmad47 23:fb5f8e018461 32 P[0][1] -= dt * P[1][1];
ahmad47 23:fb5f8e018461 33 P[1][0] -= dt * P[1][1];
ahmad47 23:fb5f8e018461 34 P[1][1] += Q_bias * dt;
ahmad47 23:fb5f8e018461 35
ahmad47 23:fb5f8e018461 36 // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
ahmad47 23:fb5f8e018461 37 // Calculate Kalman gain - Compute the Kalman gain
ahmad47 23:fb5f8e018461 38 /* Step 4 */
ahmad47 23:fb5f8e018461 39 S = P[0][0] + R_measure;
ahmad47 23:fb5f8e018461 40 /* Step 5 */
ahmad47 23:fb5f8e018461 41 K[0] = P[0][0] / S;
ahmad47 23:fb5f8e018461 42 K[1] = P[1][0] / S;
ahmad47 23:fb5f8e018461 43
ahmad47 23:fb5f8e018461 44 // Calculate angle and bias - Update estimate with measurement zk (newAngle)
ahmad47 23:fb5f8e018461 45 /* Step 3 */
ahmad47 23:fb5f8e018461 46 y = newAngle - angle;
ahmad47 23:fb5f8e018461 47 /* Step 6 */
ahmad47 23:fb5f8e018461 48 angle += K[0] * y;
ahmad47 23:fb5f8e018461 49 bias += K[1] * y;
ahmad47 23:fb5f8e018461 50
ahmad47 23:fb5f8e018461 51 // Calculate estimation error covariance - Update the error covariance
ahmad47 23:fb5f8e018461 52 /* Step 7 */
ahmad47 23:fb5f8e018461 53 P[0][0] -= K[0] * P[0][0];
ahmad47 23:fb5f8e018461 54 P[0][1] -= K[0] * P[0][1];
ahmad47 23:fb5f8e018461 55 P[1][0] -= K[1] * P[0][0];
ahmad47 23:fb5f8e018461 56 P[1][1] -= K[1] * P[0][1];
ahmad47 23:fb5f8e018461 57
ahmad47 23:fb5f8e018461 58 return angle;
ahmad47 23:fb5f8e018461 59 };
ahmad47 23:fb5f8e018461 60 void setAngle(double newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle
ahmad47 23:fb5f8e018461 61 double getRate() { return rate; }; // Return the unbiased rate
ahmad47 23:fb5f8e018461 62
ahmad47 23:fb5f8e018461 63 /* These are used to tune the Kalman filter */
ahmad47 23:fb5f8e018461 64 void setQangle(double newQ_angle) { Q_angle = newQ_angle; };
ahmad47 23:fb5f8e018461 65 void setQbias(double newQ_bias) { Q_bias = newQ_bias; };
ahmad47 23:fb5f8e018461 66 void setRmeasure(double newR_measure) { R_measure = newR_measure; };
ahmad47 23:fb5f8e018461 67
ahmad47 23:fb5f8e018461 68 double getQangle() { return Q_angle; };
ahmad47 23:fb5f8e018461 69 double getQbias() { return Q_bias; };
ahmad47 23:fb5f8e018461 70 double getRmeasure() { return R_measure; };
ahmad47 23:fb5f8e018461 71
ahmad47 23:fb5f8e018461 72 private:
ahmad47 23:fb5f8e018461 73 /* Kalman filter variables */
ahmad47 23:fb5f8e018461 74 double Q_angle; // Process noise variance for the accelerometer
ahmad47 23:fb5f8e018461 75 double Q_bias; // Process noise variance for the gyro bias
ahmad47 23:fb5f8e018461 76 double R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
ahmad47 23:fb5f8e018461 77
ahmad47 23:fb5f8e018461 78 double angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
ahmad47 23:fb5f8e018461 79 double bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
ahmad47 23:fb5f8e018461 80 double rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
ahmad47 23:fb5f8e018461 81
ahmad47 23:fb5f8e018461 82 double P[2][2]; // Error covariance matrix - This is a 2x2 matrix
ahmad47 23:fb5f8e018461 83 double K[2]; // Kalman gain - This is a 2x1 vector
ahmad47 23:fb5f8e018461 84 double y; // Angle difference
ahmad47 23:fb5f8e018461 85 double S; // Estimate error
ahmad47 23:fb5f8e018461 86 };
ahmad47 23:fb5f8e018461 87
ahmad47 23:fb5f8e018461 88 #endif
ahmad47 23:fb5f8e018461 89