Kalman.h

00001 #ifndef KALMAN_H
00002 #define KALMAN_H
00003 
00004 #include "mbed.h"
00005 #include <stdlib.h>
00006 #include <math.h>
00007 static double dt;
00008 
00009 /*PITCH*/
00010 /* Kalman filter variables*/
00011 static double Q_angle_pitch;    // Process noise variance for the accelerometer
00012 static double Q_bias_pitch;     // Process noise variance for the gyro bias
00013 static double R_measure_pitch;  // Measurement noise variance - this is actually the variance of the measurement noise
00014 
00015 static double angle_pitch;      // The angle calculated by the Kalman filter - part of the 2x1 state matrix
00016 static double bias_pitch;       // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix
00017 static double rate_pitch;       // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
00018 static double newAngle_pitch;
00019 static double newRate_pitch;
00020 
00021 static double P_pitch[2][2]; // Error covariance matrix - This is a 2x2 matrix
00022 static double K_pitch[2]; // Kalman gain - This is a 2x1 matrix
00023 static double y_pitch; // Angle difference - 1x1 matrix
00024 static double S_pitch; // Estimate error - 1x1 matrix
00025 
00026 /*ROLL*/
00027 /* Kalman filter variables*/
00028 static double Q_angle_roll;    // Process noise variance for the accelerometer
00029 static double Q_bias_roll;     // Process noise variance for the gyro bias
00030 static double R_measure_roll;  // Measurement noise variance - this is actually the variance of the measurement noise
00031 
00032 static double angle_roll;      // The angle calculated by the Kalman filter - part of the 2x1 state matrix
00033 static double bias_roll;       // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix
00034 static double rate_roll;       // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
00035 static double newAngle_roll;
00036 static double newRate_roll;
00037 
00038 static double P_roll[2][2]; // Error covariance matrix - This is a 2x2 matrix
00039 static double K_roll[2]; // Kalman gain - This is a 2x1 matrix
00040 static double y_roll; // Angle difference - 1x1 matrix
00041 static double S_roll; // Estimate error - 1x1 matrix
00042 
00043 /*Yaw*/
00044 /* Kalman filter variables*/
00045 static double Q_angle_yaw;    // Process noise variance for the accelerometer
00046 static double Q_bias_yaw;     // Process noise variance for the gyro bias
00047 static double R_measure_yaw;  // Measurement noise variance - this is actually the variance of the measurement noise
00048 
00049 static double angle_yaw;      // The angle calculated by the Kalman filter - part of the 2x1 state matrix
00050 static double bias_yaw;       // The gyro bias calculated by the Kalman filter - part of the 2x1 state matrix
00051 static double rate_yaw;       // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
00052 static double newAngle_yaw;
00053 static double newRate_yaw;
00054 
00055 static double P_yaw[2][2]; // Error covariance matrix - This is a 2x2 matrix
00056 static double K_yaw[2]; // Kalman gain - This is a 2x1 matrix
00057 static double y_yaw; // Angle difference - 1x1 matrix
00058 static double S_yaw; // Estimate error - 1x1 matrix
00059 
00060 
00061 
00062 
00063 
00064 double getRate(double *rate);
00065 
00066 void Kalman_yaw(void); 
00067 void Kalman_pitch(void); 
00068 void Kalman_roll(void); 
00069 
00070  
00071 double getPitch(double *newAngle_pitch, double *newRate_pitch, double *dt);
00072 double getRoll(double *newAngle_roll, double *newRate_roll, double *dt);
00073 double getYaw(double *newAngle_yaw, double *newRate_yaw, double *dt);
00074 
00075 
00076 
00077 
00078 void setAngle(double *newAngle, double *angle); 
00079 
00080 
00081 
00082 void setQangle(double *newQ_angle, double *Q_angle); 
00083 void setQbias(double *newQ_bias, double *Q_bias); 
00084 void setRmeasure(double newR_measure);
00085 
00086 
00087 
00088 
00089 
00090 #endif