HAVRM mine / Mbed 2 deprecated Nucleo_L3GD20_MMA7361_Kalman

kalman filter using L3GD20 and MMA7361. But this program has about 0.5s delay

Dependencies:   L3GD20 MMA7361 mbed

kalman.h@0:e60a2d5cccc2, 2016-09-28 (annotated)

Committer:
hirokimineshita
Date:
Wed Sep 28 11:12:37 2016 +0000
Revision:
0:e60a2d5cccc2
a

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hirokimineshita 0:e60a2d5cccc2 1 /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
hirokimineshita 0:e60a2d5cccc2 2
hirokimineshita 0:e60a2d5cccc2 3 This software may be distributed and modified under the terms of the GNU
hirokimineshita 0:e60a2d5cccc2 4 General Public License version 2 (GPL2) as published by the Free Software
hirokimineshita 0:e60a2d5cccc2 5 Foundation and appearing in the file GPL2.TXT included in the packaging of
hirokimineshita 0:e60a2d5cccc2 6 this file. Please note that GPL2 Section 2[b] requires that all works based
hirokimineshita 0:e60a2d5cccc2 7 on this software must also be made publicly available under the terms of
hirokimineshita 0:e60a2d5cccc2 8 the GPL2 ("Copyleft").
hirokimineshita 0:e60a2d5cccc2 9
hirokimineshita 0:e60a2d5cccc2 10 Contact information
hirokimineshita 0:e60a2d5cccc2 11 -------------------
hirokimineshita 0:e60a2d5cccc2 12
hirokimineshita 0:e60a2d5cccc2 13 Kristian Lauszus, TKJ Electronics
hirokimineshita 0:e60a2d5cccc2 14 Web : http://www.tkjelectronics.com
hirokimineshita 0:e60a2d5cccc2 15 e-mail : kristianl@tkjelectronics.com
hirokimineshita 0:e60a2d5cccc2 16 */
hirokimineshita 0:e60a2d5cccc2 17
hirokimineshita 0:e60a2d5cccc2 18 #ifndef _Kalman_h
hirokimineshita 0:e60a2d5cccc2 19 #define _Kalman_h
hirokimineshita 0:e60a2d5cccc2 20
hirokimineshita 0:e60a2d5cccc2 21 class Kalman {
hirokimineshita 0:e60a2d5cccc2 22 public:
hirokimineshita 0:e60a2d5cccc2 23 Kalman() {
hirokimineshita 0:e60a2d5cccc2 24 /* We will set the variables like so, these can also be tuned by the user */
hirokimineshita 0:e60a2d5cccc2 25 Q_angle = 0.001f;
hirokimineshita 0:e60a2d5cccc2 26 Q_bias = 0.003f;
hirokimineshita 0:e60a2d5cccc2 27 R_measure = 0.03f;
hirokimineshita 0:e60a2d5cccc2 28
hirokimineshita 0:e60a2d5cccc2 29 angle = 0.0f; // Reset the angle
hirokimineshita 0:e60a2d5cccc2 30 bias = 0.0f; // Reset bias
hirokimineshita 0:e60a2d5cccc2 31
hirokimineshita 0:e60a2d5cccc2 32 P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
hirokimineshita 0:e60a2d5cccc2 33 P[0][1] = 0.0f;
hirokimineshita 0:e60a2d5cccc2 34 P[1][0] = 0.0f;
hirokimineshita 0:e60a2d5cccc2 35 P[1][1] = 0.0f;
hirokimineshita 0:e60a2d5cccc2 36 };
hirokimineshita 0:e60a2d5cccc2 37
hirokimineshita 0:e60a2d5cccc2 38
hirokimineshita 0:e60a2d5cccc2 39 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
hirokimineshita 0:e60a2d5cccc2 40 float getAngle(float newAngle, float newRate, float dt) {
hirokimineshita 0:e60a2d5cccc2 41 // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
hirokimineshita 0:e60a2d5cccc2 42 // Modified by Kristian Lauszus
hirokimineshita 0:e60a2d5cccc2 43 // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
hirokimineshita 0:e60a2d5cccc2 44
hirokimineshita 0:e60a2d5cccc2 45 // Discrete Kalman filter time update equations - Time Update ("Predict")
hirokimineshita 0:e60a2d5cccc2 46 // Update xhat - Project the state ahead
hirokimineshita 0:e60a2d5cccc2 47 /* Step 1 */
hirokimineshita 0:e60a2d5cccc2 48 rate = newRate - bias;
hirokimineshita 0:e60a2d5cccc2 49 angle += dt * rate;
hirokimineshita 0:e60a2d5cccc2 50
hirokimineshita 0:e60a2d5cccc2 51 // Update estimation error covariance - Project the error covariance ahead
hirokimineshita 0:e60a2d5cccc2 52 /* Step 2 */
hirokimineshita 0:e60a2d5cccc2 53 P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
hirokimineshita 0:e60a2d5cccc2 54 P[0][1] -= dt * P[1][1];
hirokimineshita 0:e60a2d5cccc2 55 P[1][0] -= dt * P[1][1];
hirokimineshita 0:e60a2d5cccc2 56 P[1][1] += Q_bias * dt;
hirokimineshita 0:e60a2d5cccc2 57
hirokimineshita 0:e60a2d5cccc2 58 // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
hirokimineshita 0:e60a2d5cccc2 59 // Calculate Kalman gain - Compute the Kalman gain
hirokimineshita 0:e60a2d5cccc2 60 /* Step 4 */
hirokimineshita 0:e60a2d5cccc2 61 float S = P[0][0] + R_measure; // Estimate error
hirokimineshita 0:e60a2d5cccc2 62 /* Step 5 */
hirokimineshita 0:e60a2d5cccc2 63 float K[2]; // Kalman gain - This is a 2x1 vector
hirokimineshita 0:e60a2d5cccc2 64 K[0] = P[0][0] / S;
hirokimineshita 0:e60a2d5cccc2 65 K[1] = P[1][0] / S;
hirokimineshita 0:e60a2d5cccc2 66
hirokimineshita 0:e60a2d5cccc2 67 // Calculate angle and bias - Update estimate with measurement zk (newAngle)
hirokimineshita 0:e60a2d5cccc2 68 /* Step 3 */
hirokimineshita 0:e60a2d5cccc2 69 float y = newAngle - angle; // Angle difference
hirokimineshita 0:e60a2d5cccc2 70 /* Step 6 */
hirokimineshita 0:e60a2d5cccc2 71 angle += K[0] * y;
hirokimineshita 0:e60a2d5cccc2 72 bias += K[1] * y;
hirokimineshita 0:e60a2d5cccc2 73
hirokimineshita 0:e60a2d5cccc2 74 // Calculate estimation error covariance - Update the error covariance
hirokimineshita 0:e60a2d5cccc2 75 /* Step 7 */
hirokimineshita 0:e60a2d5cccc2 76 float P00_temp = P[0][0];
hirokimineshita 0:e60a2d5cccc2 77 float P01_temp = P[0][1];
hirokimineshita 0:e60a2d5cccc2 78
hirokimineshita 0:e60a2d5cccc2 79 P[0][0] -= K[0] * P00_temp;
hirokimineshita 0:e60a2d5cccc2 80 P[0][1] -= K[0] * P01_temp;
hirokimineshita 0:e60a2d5cccc2 81 P[1][0] -= K[1] * P00_temp;
hirokimineshita 0:e60a2d5cccc2 82 P[1][1] -= K[1] * P01_temp;
hirokimineshita 0:e60a2d5cccc2 83
hirokimineshita 0:e60a2d5cccc2 84
hirokimineshita 0:e60a2d5cccc2 85 return angle;
hirokimineshita 0:e60a2d5cccc2 86 };
hirokimineshita 0:e60a2d5cccc2 87
hirokimineshita 0:e60a2d5cccc2 88 void setAngle(float newAngle) { angle = newAngle; }; // Used to set angle, this should be set as the starting angle
hirokimineshita 0:e60a2d5cccc2 89 float getRate() { return rate; }; // Return the unbiased rate
hirokimineshita 0:e60a2d5cccc2 90
hirokimineshita 0:e60a2d5cccc2 91 /* These are used to tune the Kalman filter */
hirokimineshita 0:e60a2d5cccc2 92 void setQangle(float newQ_angle) { Q_angle = newQ_angle; };
hirokimineshita 0:e60a2d5cccc2 93 void setQbias(float newQ_bias) { Q_bias = newQ_bias; };
hirokimineshita 0:e60a2d5cccc2 94 void setRmeasure(float newR_measure) { R_measure = newR_measure; };
hirokimineshita 0:e60a2d5cccc2 95
hirokimineshita 0:e60a2d5cccc2 96 float getQangle() { return Q_angle; };
hirokimineshita 0:e60a2d5cccc2 97 float getQbias() { return Q_bias; };
hirokimineshita 0:e60a2d5cccc2 98 float getRmeasure() { return R_measure; };
hirokimineshita 0:e60a2d5cccc2 99
hirokimineshita 0:e60a2d5cccc2 100 private:
hirokimineshita 0:e60a2d5cccc2 101 /* Kalman filter variables */
hirokimineshita 0:e60a2d5cccc2 102 float Q_angle; // Process noise variance for the accelerometer
hirokimineshita 0:e60a2d5cccc2 103 float Q_bias; // Process noise variance for the gyro bias
hirokimineshita 0:e60a2d5cccc2 104 float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
hirokimineshita 0:e60a2d5cccc2 105
hirokimineshita 0:e60a2d5cccc2 106 float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
hirokimineshita 0:e60a2d5cccc2 107 float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
hirokimineshita 0:e60a2d5cccc2 108 float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
hirokimineshita 0:e60a2d5cccc2 109
hirokimineshita 0:e60a2d5cccc2 110 float P[2][2]; // Error covariance matrix - This is a 2x2 matrix
hirokimineshita 0:e60a2d5cccc2 111 };
hirokimineshita 0:e60a2d5cccc2 112
hirokimineshita 0:e60a2d5cccc2 113 #endif