KalmanFilter.h

00001 /*
00002  * KalmanFilter.h
00003  *
00004  *  Created on: Feb 19, 2010
00005  *      Author: uscr
00006  */
00007 
00008 #ifndef KALMANFILTER_H_
00009 #define KALMANFILTER_H_
00010 
00011 /*
00012 struct KalmanFilterData {
00013 
00014         double X, Boff;
00015 
00016         double gyroVar, accelVar, gyroAlpha, accelAlpha, B, P;
00017 
00018         double gyroLP, accelLP;
00019 };*/
00020 
00021 struct KalmanFilterData {
00022         double x_0, x_1;
00023         double innov, cov;
00024         double K_0, K_1;
00025         double P_00, P_01, P_10, P_11;
00026 
00027         double Sz, Sw_00, Sw_11;
00028 
00029 };
00030 
00031 
00032 void initializeKalmanFilter(struct KalmanFilterData * filter, double sz, double sw0, double sw1);
00033 void updateKalmanFilter(struct KalmanFilterData * filter, double adj_gyro, double adj_accel, double delta_t);
00034 double getCircularBufferWeightedMean(double * data, double * weights, int startVal, int numVals, int maxPoints);
00035 
00036 
00037 #endif /* KALMANFILTER_H_ */