Einstein Filho / Mbed OS MANGUEBAJA2019_FRONT2

Mangue Baja team's code to frontal ECU

Kalman/Kalman.cpp@0:12fb9cbcabcc, 2019-07-24 (annotated)

Committer:
einsteingustavo
Date:
Wed Jul 24 20:03:52 2019 +0000
Revision:
0:12fb9cbcabcc
Mangue Baja team's code to frontal ECU

Who changed what in which revision?

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