ESE350 project, Spring 2016, University of Pennsylvania
Dependencies: Adafruit9-DOf Receiver mbed-rtos mbed
Kalman.cpp@34:eaea0ae92dfa, 2016-04-24 (annotated)
- Committer:
- ivo_david_michelle
- Date:
- Sun Apr 24 17:15:20 2016 +0000
- Revision:
- 34:eaea0ae92dfa
testing with kalman
Who changed what in which revision?
User | Revision | Line number | New 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; }; |