Kalman.cpp

00001 #include "Kalman.h"
00002 #include "mbed.h"
00003 
00004 /*PITCH*/
00005 /*PITCH*/
00006 /*PITCH*/
00007 
00008 void Kalman_pitch(void) 
00009 {
00010     /* We will set the varibles like so, these can also be tuned by the user */
00011     Q_angle_pitch = 0.1;
00012     Q_bias_pitch = 0.003;
00013     R_measure_pitch = 0.1;
00014     
00015     angle_pitch = 1.2;
00016     bias_pitch = -0.4; // Reset bias
00017     // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), 
00018     // the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
00019     P_pitch[0][0] = 0;
00020     P_pitch[0][1] = 0;
00021     P_pitch[1][0] = 0;
00022     P_pitch[1][1] = 0;
00023 }
00024 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
00025 double getPitch(double *newAngle_pitch, double *newRate_pitch, double *dt) 
00026 {
00027     // Discrete Kalman filter time update equations - Time Update ("Predict")
00028     // Update xhat - Project the state ahead
00029     /* Step 1 */
00030     rate_pitch = (*newRate_pitch) - bias_pitch;
00031     angle_pitch += (*dt) * rate_pitch;
00032     
00033     // Update estimation error covariance - Project the error covariance ahead
00034     /* Step 2 */
00035     P_pitch[0][0] += (*dt) * ((*dt) * P_pitch[1][1] - P_pitch[0][1] - P_pitch[1][0] + Q_angle_pitch);
00036     P_pitch[0][1] -= (*dt) * P_pitch[1][1];
00037     P_pitch[1][0] -= (*dt) * P_pitch[1][1];
00038     P_pitch[1][1] += Q_bias_pitch * (*dt);
00039     
00040     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
00041     // Calculate Kalman gain - Compute the Kalman gain
00042     /* Step 4 */
00043     S_pitch = P_pitch[0][0] + R_measure_pitch;
00044     /* Step 5 */
00045     K_pitch[0] = P_pitch[0][0] / S_pitch;
00046     K_pitch[1] = P_pitch[1][0] / S_pitch;
00047     
00048     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
00049     /* Step 3 */
00050     y_pitch = (*newAngle_pitch) - angle_pitch;
00051     /* Step 6 */
00052     angle_pitch += K_pitch[0] * y_pitch;
00053     bias_pitch += K_pitch[1] * y_pitch;
00054     
00055     // Calculate estimation error covariance - Update the error covariance
00056     /* Step 7 */
00057     P_pitch[0][0] -= K_pitch[0] * P_pitch[0][0];
00058     P_pitch[0][1] -= K_pitch[0] * P_pitch[0][1];
00059     P_pitch[1][0] -= K_pitch[1] * P_pitch[0][0];
00060     P_pitch[1][1] -= K_pitch[1] * P_pitch[0][1];
00061     
00062     return angle_pitch;
00063 }
00064 
00065 /*YAW*/
00066 /*YAW*/
00067 /*YAW*/
00068 
00069 void Kalman_yaw(void) 
00070 {
00071     /* We will set the varibles like so, these can also be tuned by the user */
00072     Q_angle_yaw = 0.001;
00073     Q_bias_yaw = 0.003;
00074     R_measure_yaw = 0.03;
00075     
00076     bias_yaw = 0; // Reset bias
00077     // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), 
00078     // the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
00079     P_yaw[0][0] = 0;
00080     P_yaw[0][1] = 0;
00081     P_yaw[1][0] = 0;
00082     P_yaw[1][1] = 0;
00083 }
00084 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
00085 double getYaw(double *newAngle_yaw, double *newRate_yaw, double *dt) 
00086 {
00087     // Discrete Kalman filter time update equations - Time Update ("Predict")
00088     // Update xhat - Project the state ahead
00089     /* Step 1 */
00090     rate_yaw = (*newRate_yaw) - bias_yaw;
00091     angle_yaw += (*dt) * rate_yaw;
00092     
00093     (*newAngle_yaw) = angle_yaw;
00094     
00095     // Update estimation error covariance - Project the error covariance ahead
00096     /* Step 2 */
00097     P_yaw[0][0] += (*dt) * ((*dt) * P_yaw[1][1] - P_yaw[0][1] - P_yaw[1][0] + Q_angle_yaw);
00098     P_yaw[0][1] -= (*dt) * P_yaw[1][1];
00099     P_yaw[1][0] -= (*dt) * P_yaw[1][1];
00100     P_yaw[1][1] += Q_bias_yaw * (*dt);
00101     
00102     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
00103     // Calculate Kalman gain - Compute the Kalman gain
00104     /* Step 4 */
00105     S_yaw = P_yaw[0][0] + R_measure_yaw;
00106     /* Step 5 */
00107     K_yaw[0] = P_yaw[0][0] / S_yaw;
00108     K_yaw[1] = P_yaw[1][0] / S_yaw;
00109     
00110     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
00111     /* Step 3 */
00112     y_yaw = (*newAngle_yaw) - angle_yaw;
00113     /* Step 6 */
00114     angle_yaw += K_yaw[0] * y_yaw;
00115     bias_yaw += K_yaw[1] * y_yaw;
00116     
00117     // Calculate estimation error covariance - Update the error covariance
00118     /* Step 7 */
00119     P_yaw[0][0] -= K_yaw[0] * P_yaw[0][0];
00120     P_yaw[0][1] -= K_yaw[0] * P_yaw[0][1];
00121     P_yaw[1][0] -= K_yaw[1] * P_yaw[0][0];
00122     P_yaw[1][1] -= K_yaw[1] * P_yaw[0][1];
00123     
00124     return angle_yaw;
00125 }
00126 
00127 
00128 void Kalman_roll(void) 
00129 {
00130     /* We will set the varibles like so, these can also be tuned by the user */
00131     Q_angle_roll = 0.001;
00132     Q_bias_roll = 0.1;
00133     R_measure_roll = 0.1;
00134     
00135     angle_roll = 1.2;
00136     bias_roll = 1.2; // Reset bias
00137     // Since we assume tha the bias is 0 and we know the starting angle (use setAngle), 
00138     // the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
00139     P_roll[0][0] = 0;
00140     P_roll[0][1] = 0;
00141     P_roll[1][0] = 0;
00142     P_roll[1][1] = 0;
00143 }
00144 
00145 /*ROLL*/
00146 /*ROLL*/
00147 /*ROLL*/
00148 
00149 // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
00150 double getRoll(double *newAngle_roll, double *newRate_roll, double *dt) 
00151 {
00152     // Discrete Kalman filter time update equations - Time Update ("Predict")
00153     // Update xhat - Project the state ahead
00154     /* Step 1 */
00155     rate_roll = (*newRate_roll) - bias_roll;
00156     angle_roll += (*dt) * rate_roll;
00157     
00158     // Update estimation error covariance - Project the error covariance ahead
00159     /* Step 2 */
00160     P_roll[0][0] += (*dt) * ((*dt) * P_roll[1][1] - P_roll[0][1] - P_roll[1][0] + Q_angle_roll);
00161     P_roll[0][1] -= (*dt) * P_roll[1][1];
00162     P_roll[1][0] -= (*dt) * P_roll[1][1];
00163     P_roll[1][1] += Q_bias_roll * (*dt);
00164     
00165     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
00166     // Calculate Kalman gain - Compute the Kalman gain
00167     /* Step 4 */
00168     S_roll = P_roll[0][0] + R_measure_roll;
00169     /* Step 5 */
00170     K_roll[0] = P_roll[0][0] / S_roll;
00171     K_roll[1] = P_roll[1][0] / S_roll;
00172     
00173     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
00174     /* Step 3 */
00175     y_roll = (*newAngle_roll) - angle_roll;
00176     /* Step 6 */
00177     angle_roll += K_roll[0] * y_roll;
00178     bias_roll += K_roll[1] * y_roll;
00179     
00180     // Calculate estimation error covariance - Update the error covariance
00181     /* Step 7 */
00182     P_roll[0][0] -= K_roll[0] * P_roll[0][0];
00183     P_roll[0][1] -= K_roll[0] * P_roll[0][1];
00184     P_roll[1][0] -= K_roll[1] * P_roll[0][0];
00185     P_roll[1][1] -= K_roll[1] * P_roll[0][1];
00186     
00187     return angle_roll;
00188 }
00189 
00190 
00191 
00192 
00193 
00194 
00195 
00196 
00197 // Used to set angle, this should be set as the starting angle
00198 void setAngle(double *newAngle, double *angle) 
00199 { 
00200     (*angle) = (*newAngle); 
00201 }
00202 // Return the unbiased rate
00203 double getRate(double *rate) 
00204 { 
00205     return (*rate); 
00206 }
00207 
00208 /* These are used to tune the Kalman filter */
00209 void setQangle(double *newQ_angle, double *Q_angle) 
00210 { 
00211     (*Q_angle) = (*newQ_angle); 
00212 }
00213 void setQbias(double *newQ_bias, double *Q_bias) 
00214 { 
00215     (*Q_bias) = (*newQ_bias); 
00216 }
00217 void setRmeasure(double *newR_measure, double *R_measure) 
00218 {
00219     (*R_measure) = (*newR_measure); 
00220 }
00221 
00222