Simple Kalman Filter

simple_kalman.cpp

#include <iostream>
#include <cmath>

using namespace std;

int main() {

	float pre[255] = {-0.054337, -0.036224, -0.036224, -0.054337, -0.054337, -0.036224, -0.054337, -0.054337, -0.072449, -0.126785, -0.144897, -0.144897, -0.163010, -0.199234, -0.199234, -0.235458, -0.271683, -0.326019, -0.380356, -0.416580, -0.470917, -0.507141, -0.561478, -0.597702, -0.652039, -0.688263, -0.706375, -0.742599, -0.833160, -0.833160, -0.869385, -0.887497, -0.905609, -0.923721, -0.941833, -0.923721, -0.978058, -0.996170, -0.978058, -0.978058, -1.032394, -0.996170, -1.050507, -1.014282, -1.014282, -1.014282, -0.959946, -0.959946, -0.923721, -0.869385, -0.869385, -0.778824, -0.815048, -0.796936, -0.706375, -0.579590, -0.561478, -0.489029, -0.507141, -0.452805, -0.434692, -0.307907, -0.380356, -0.253571, -0.307907, -0.217346, -0.126785, -0.108673, -0.163010, -0.018112, -0.018112, 0.054337, 0.126785, 0.054337, 0.235458, 0.434692, 0.579590, 0.688263, 0.742599, 0.724487, 0.833160, 0.887497, 0.652039, 0.561478, 0.724487, 0.633926, 0.615814, 0.543365, 0.434692, 0.507141, -0.036224, 0.253571, 0.489029, 0.144897, 0.163010, 0.362244, 0.036224, -0.072449, 0.199234, -0.217346, 0.018112, -0.344131, -0.307907, -0.036224, -0.470917, -0.307907, -0.724487, -0.543365, -0.289795, -0.615814, -0.434692, -0.796936, -0.579590, -0.416580, -0.597702, -0.380356, -0.688263, -0.742599, -0.978058, -0.760712, -0.742599, -0.869385, -0.778824, -0.978058, -0.941833, -1.249741, -0.978058, -0.978058, -0.996170, -1.122955, -1.068619, -1.068619, -1.050507, -0.905609, -1.014282, -0.923721, -1.050507, -0.923721, -1.104843, -0.887497, -1.032394, -0.978058, -1.032394, -0.941833, -1.068619, -0.778824, -0.597702, -0.670151, -0.398468, -0.525253, -0.235458, -0.380356, -0.054337, -0.253571, 0.072449, -0.108673, 0.362244, 0.181122, 0.597702, 0.271683, 0.543365, 0.416580, 0.706375, 0.543365, 0.815048, 0.724487, 0.941833, 0.796936, 1.050507, 0.851273, 1.068619, 0.815048, 1.068619, 0.941833, 0.652039, 1.177292, 0.887497, 1.141068, 0.887497, 1.213516, 0.887497, 1.231628, 0.869385, 1.141068, 0.887497, 1.122955, 0.833160, 1.249741, 0.815048, 1.032394, 0.869385, 1.086731, 0.869385, 0.996170, 0.941833, 0.706375, 1.104843, 0.978058, 0.597702, 1.014282, 0.742599, -0.018112, 0.633926, 0.217346, 0.434692, 0.380356, 0.525253, 0.615814, 0.905609, 0.996170, 0.996170, 0.905609, 0.633926, 0.108673, -0.126785, 0.144897, 0.362244, 0.434692, 0.434692, 0.597702, 0.344131, 0.199234, 0.380356, -0.036224, -0.054337, -0.036224, -0.036224, -0.036224, -0.036224, -0.036224, -0.036224, -0.036224, -0.036224, -0.036224, -0.036224, -0.018112, -0.036224, -0.018112, -0.018112, -0.036224, -0.036224, -0.036224, -0.036224, -0.018112, -0.036224, -0.036224, -0.018112, -0.036224, -0.036224, -0.036224, -0.036224, -0.036224, -0.036224, -0.036224, -0.018112};
	
	int iterations = 255;
	
	double q = 0.0001; // Modify this
	double r = 0.0008; // Modify this
	
	double xhat[255] = {};
	double p[255] = {};
	double xhatminus[255] = {};
	double pminus[255] = {};
	double k[255] = {};
	
	xhat[0] = 0.0;
	p[0] = 1.0;
	
	for(int i = 1; i < iterations; i++) {
		xhatminus[i] = xhat[i-1];
		pminus[i] = p[i-1] + q;
		
		k[i] = pminus[i]/(pminus[i]+r);
		xhat[i] = xhatminus[i]+k[i]*(pre[i]-xhatminus[i]);
		p[i] = (1-k[i])*pminus[i];
		
		cout << endl << xhat[i];
	}
	
	return 0;

}