Computes Euler angles
Fork of X_NUCLEO_COMMON by
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?
User | Revision | Line number | New 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 |