KalmanFilter.cc

00001 /*
00002  * KalmanFilter.c
00003  *
00004  *  Created on: Feb 19, 2010
00005  *      Author: uscr
00006  */
00007 
00008 #include "KalmanFilter.h"
00009 #include <cstdio>
00010 
00011 struct KalmanFilterData pitchFilter, rollFilter;
00012 
00013 void initializeKalmanFilter(struct KalmanFilterData * filter, double sz, double sw0, double sw1) {
00014 
00015         filter->x_0 = 0.0;
00016         filter->x_1 = 0.0;
00017         filter->innov = 0.0;
00018         filter->cov = 0.0;
00019         filter->K_0 = 0.0;
00020         filter->K_1 = 0.0;
00021         filter->P_00 = 1.0;
00022         filter->P_01 = 0.0;
00023         filter->P_10 = 0.0;
00024         filter->P_11 = 1.0;
00025 
00026         filter->Sz = sz;
00027         filter->Sw_00 = sw0;
00028         filter->Sw_11 = sw1;
00029 }
00030 
00031 
00032 void updateKalmanFilter(struct KalmanFilterData * filter, double adj_gyro, double accel_angle, double delta_t) {
00033 
00034         //Update the state of our model
00035         //x(k+1) = A*x(k) + B*u
00036         filter->x_0 += delta_t*(adj_gyro - filter->x_1);
00037 
00038         //Get the "innovation" - AKA the diff between what we think the value is and what our second
00039         // data source says the value is
00040         filter->innov = accel_angle - filter->x_0;
00041 
00042         //Get the covariance of the measurement
00043         filter->cov = filter->P_00 + filter->Sz;
00044 
00045         //Get the kalman gain
00046         filter->K_0 = (filter->P_00 - delta_t*(filter->P_10))/(filter->cov);
00047         filter->K_1 = (filter->P_10)/(filter->cov);
00048 
00049         //Correct the prediction of the state
00050         filter->x_0 += (filter->K_0)*(filter->innov);
00051         filter->x_1 += (filter->K_1)*(filter->innov);
00052 
00053         //Get the covariance of the prediction error
00054         double new_P_00 = (filter->P_00)*(1 - filter->K_0) +
00055                 (filter->P_01)*delta_t*( filter->K_0 - 1) +
00056                 (filter->P_10)*(-delta_t) +
00057                 (filter->P_11)*delta_t*delta_t;
00058         double new_P_01 = (filter->P_01)*(1 - filter->K_0) -
00059                         delta_t*(filter->P_11);
00060         double new_P_10 = filter->P_10 +
00061                         delta_t*((filter->K_1)*(filter->P_01) - filter->P_11) -
00062                         (filter->K_1)*(filter->P_00);
00063         double new_P_11 = filter->P_11 -
00064                         (filter->K_1)*(filter->P_01);
00065 
00066         //add in the gyro variances:
00067         filter->P_00 = new_P_00 + filter->Sw_00;
00068         filter->P_01 = new_P_01;
00069         filter->P_10 = new_P_10;
00070         filter->P_11 = new_P_11 + filter->Sw_11;
00071 }
00072 
00073 
00074 //weights should be a double array of size numVals
00075 // they do not need to sum to 1
00076 double getCircularBufferWeightedMean(double * data, double * weights, int startVal, int numVals, int maxPoints) {
00077         double mean = 0;
00078         double weightSum = 0;
00079 
00080         for (int i = startVal, j = 0; j < numVals; i++, j++) {
00081                 mean += weights[j]*data[i];
00082                 weightSum += weights[j];
00083                 if (i + 1 == maxPoints)
00084                         i = -1;
00085         }
00086 
00087         return mean / weightSum;
00088 }
00089 
00090