David Lakata / Mbed 2 deprecated QuadTrio

ESE350 project, Spring 2016, University of Pennsylvania

Dependencies:   Adafruit9-DOf Receiver mbed-rtos mbed

Kalman.cpp@51:b6d76a4dfae8, 2016-05-07 (annotated)

Committer:
ivo_david_michelle
Date:
Sat May 07 17:47:22 2016 +0000
Revision:
51:b6d76a4dfae8
Parent:
34:eaea0ae92dfa 
publishing

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ivo_david_michelle 34:eaea0ae92dfa 1 #include "Kalman.h"
ivo_david_michelle 34:eaea0ae92dfa 2
ivo_david_michelle 34:eaea0ae92dfa 3 Kalman::Kalman() {
ivo_david_michelle 34:eaea0ae92dfa 4 /* We will set the variables like so, these can also be tuned by the user */
ivo_david_michelle 34:eaea0ae92dfa 5 Q_angle = 0.001f;
ivo_david_michelle 34:eaea0ae92dfa 6 Q_bias = 0.003f;
ivo_david_michelle 34:eaea0ae92dfa 7 R_measure = 0.03f;
ivo_david_michelle 34:eaea0ae92dfa 8
ivo_david_michelle 34:eaea0ae92dfa 9 angle = 0.0f; // Reset the angle
ivo_david_michelle 34:eaea0ae92dfa 10 bias = 0.0f; // Reset bias
ivo_david_michelle 34:eaea0ae92dfa 11
ivo_david_michelle 34:eaea0ae92dfa 12 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
ivo_david_michelle 34:eaea0ae92dfa 13 P[0][1] = 0.0f;
ivo_david_michelle 34:eaea0ae92dfa 14 P[1][0] = 0.0f;
ivo_david_michelle 34:eaea0ae92dfa 15 P[1][1] = 0.0f;
ivo_david_michelle 34:eaea0ae92dfa 16 };
ivo_david_michelle 34:eaea0ae92dfa 17
ivo_david_michelle 34:eaea0ae92dfa 18 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
ivo_david_michelle 34:eaea0ae92dfa 19 float Kalman::getAngle(float newAngle, float newRate, float dt) {
ivo_david_michelle 34:eaea0ae92dfa 20 // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
ivo_david_michelle 34:eaea0ae92dfa 21 // Modified by Kristian Lauszus
ivo_david_michelle 34:eaea0ae92dfa 22 // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
ivo_david_michelle 34:eaea0ae92dfa 23
ivo_david_michelle 34:eaea0ae92dfa 24 // Discrete Kalman filter time update equations - Time Update ("Predict")
ivo_david_michelle 34:eaea0ae92dfa 25 // Update xhat - Project the state ahead
ivo_david_michelle 34:eaea0ae92dfa 26 /* Step 1 */
ivo_david_michelle 34:eaea0ae92dfa 27 rate = newRate - bias;
ivo_david_michelle 34:eaea0ae92dfa 28 angle += dt * rate;
ivo_david_michelle 34:eaea0ae92dfa 29
ivo_david_michelle 34:eaea0ae92dfa 30 // Update estimation error covariance - Project the error covariance ahead
ivo_david_michelle 34:eaea0ae92dfa 31 /* Step 2 */
ivo_david_michelle 34:eaea0ae92dfa 32 P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
ivo_david_michelle 34:eaea0ae92dfa 33 P[0][1] -= dt * P[1][1];
ivo_david_michelle 34:eaea0ae92dfa 34 P[1][0] -= dt * P[1][1];
ivo_david_michelle 34:eaea0ae92dfa 35 P[1][1] += Q_bias * dt;
ivo_david_michelle 34:eaea0ae92dfa 36
ivo_david_michelle 34:eaea0ae92dfa 37 // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
ivo_david_michelle 34:eaea0ae92dfa 38 // Calculate Kalman gain - Compute the Kalman gain
ivo_david_michelle 34:eaea0ae92dfa 39 /* Step 4 */
ivo_david_michelle 34:eaea0ae92dfa 40 float S = P[0][0] + R_measure; // Estimate error
ivo_david_michelle 34:eaea0ae92dfa 41 /* Step 5 */
ivo_david_michelle 34:eaea0ae92dfa 42 float K[2]; // Kalman gain - This is a 2x1 vector
ivo_david_michelle 34:eaea0ae92dfa 43 K[0] = P[0][0] / S;
ivo_david_michelle 34:eaea0ae92dfa 44 K[1] = P[1][0] / S;
ivo_david_michelle 34:eaea0ae92dfa 45
ivo_david_michelle 34:eaea0ae92dfa 46 // Calculate angle and bias - Update estimate with measurement zk (newAngle)
ivo_david_michelle 34:eaea0ae92dfa 47 /* Step 3 */
ivo_david_michelle 34:eaea0ae92dfa 48 float y = newAngle - angle; // Angle difference
ivo_david_michelle 34:eaea0ae92dfa 49 /* Step 6 */
ivo_david_michelle 34:eaea0ae92dfa 50 angle += K[0] * y;
ivo_david_michelle 34:eaea0ae92dfa 51 bias += K[1] * y;
ivo_david_michelle 34:eaea0ae92dfa 52
ivo_david_michelle 34:eaea0ae92dfa 53 // Calculate estimation error covariance - Update the error covariance
ivo_david_michelle 34:eaea0ae92dfa 54 /* Step 7 */
ivo_david_michelle 34:eaea0ae92dfa 55 float P00_temp = P[0][0];
ivo_david_michelle 34:eaea0ae92dfa 56 float P01_temp = P[0][1];
ivo_david_michelle 34:eaea0ae92dfa 57
ivo_david_michelle 34:eaea0ae92dfa 58 P[0][0] -= K[0] * P00_temp;
ivo_david_michelle 34:eaea0ae92dfa 59 P[0][1] -= K[0] * P01_temp;
ivo_david_michelle 34:eaea0ae92dfa 60 P[1][0] -= K[1] * P00_temp;
ivo_david_michelle 34:eaea0ae92dfa 61 P[1][1] -= K[1] * P01_temp;
ivo_david_michelle 34:eaea0ae92dfa 62
ivo_david_michelle 34:eaea0ae92dfa 63 return angle;
ivo_david_michelle 34:eaea0ae92dfa 64 };
ivo_david_michelle 34:eaea0ae92dfa 65
ivo_david_michelle 34:eaea0ae92dfa 66 void Kalman::setAngle(float angle) { this->angle = angle; }; // Used to set angle, this should be set as the starting angle
ivo_david_michelle 34:eaea0ae92dfa 67 float Kalman::getRate() { return this->rate; }; // Return the unbiased rate
ivo_david_michelle 34:eaea0ae92dfa 68
ivo_david_michelle 34:eaea0ae92dfa 69 /* These are used to tune the Kalman filter */
ivo_david_michelle 34:eaea0ae92dfa 70 void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; };
ivo_david_michelle 34:eaea0ae92dfa 71 void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; };
ivo_david_michelle 34:eaea0ae92dfa 72 void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; };
ivo_david_michelle 34:eaea0ae92dfa 73
ivo_david_michelle 34:eaea0ae92dfa 74 float Kalman::getQangle() { return this->Q_angle; };
ivo_david_michelle 34:eaea0ae92dfa 75 float Kalman::getQbias() { return this->Q_bias; };
ivo_david_michelle 34:eaea0ae92dfa 76 float Kalman::getRmeasure() { return this->R_measure; };