Kalman.cpp

00001 #include "Kalman.h"
00002 
00003 Kalman::Kalman() 
00004 {
00005     /* We will set the varibles like so, these can also be tuned by the user */
00006     Q_angle = 0.001;
00007     Q_bias = 0.003;
00008     R_measure = 0.03;
00009     
00010     bias = 0; // Reset bias
00011     // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), 
00012     // the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
00013     P[0][0] = 0;
00014     P[0][1] = 0;
00015     P[1][0] = 0;
00016     P[1][1] = 0;
00017 }
00018 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
00019 double Kalman::getAngle(double newAngle, double newRate, double dt) 
00020 {
00021     // Discrete Kalman filter time update equations - Time Update ("Predict")
00022     // Update xhat - Project the state ahead
00023     /* Step 1 */
00024     rate = newRate - bias;
00025     angle += dt * rate;
00026     
00027     // Update estimation error covariance - Project the error covariance ahead
00028     /* Step 2 */
00029     P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
00030     P[0][1] -= dt * P[1][1];
00031     P[1][0] -= dt * P[1][1];
00032     P[1][1] += Q_bias * dt;
00033     
00034     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
00035     // Calculate Kalman gain - Compute the Kalman gain
00036     /* Step 4 */
00037     S = P[0][0] + R_measure;
00038     /* Step 5 */
00039     K[0] = P[0][0] / S;
00040     K[1] = P[1][0] / S;
00041     
00042     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
00043     /* Step 3 */
00044     y = newAngle - angle;
00045     /* Step 6 */
00046     angle += K[0] * y;
00047     bias += K[1] * y;
00048     
00049     // Calculate estimation error covariance - Update the error covariance
00050     /* Step 7 */
00051     P[0][0] -= K[0] * P[0][0];
00052     P[0][1] -= K[0] * P[0][1];
00053     P[1][0] -= K[1] * P[0][0];
00054     P[1][1] -= K[1] * P[0][1];
00055     
00056     return angle;
00057 }
00058 // Used to set angle, this should be set as the starting angle
00059 void Kalman::setAngle(double newAngle) 
00060 { 
00061     angle = newAngle; 
00062 }
00063 // Return the unbiased rate
00064 double Kalman::getRate() 
00065 { 
00066     return rate; 
00067 }
00068 
00069 /* These are used to tune the Kalman filter */
00070 void Kalman::setQangle(double newQ_angle) 
00071 { 
00072     Q_angle = newQ_angle; 
00073 }
00074 void Kalman::setQbias(double newQ_bias) 
00075 { 
00076     Q_bias = newQ_bias; 
00077 }
00078 void Kalman::setRmeasure(double newR_measure) 
00079 {
00080     R_measure = newR_measure; 
00081 }