A very simple Kalman Filter

SimpleKalmanFilter.cpp

#include<iostream>
#include<random>
#include<fstream>
#include<vector>
#include<chrono>

using namespace std;

float exampleFunction(float x){
	return x;
}
// Initialize p = 1 and g = 1
float kalman(float unfiltered,float last_filtered,float &p,float &g, float r){
	float filter_estimate = exampleFunction(last_filtered);

	g = p / (p + r);	
	p = (1 - g) * p;
	return filter_estimate + g * (unfiltered - filter_estimate);
}

// The main function only tests the filter
float main(){

	float noise_range = 50;
	default_random_engine dre;
	chrono::system_clock::time_point tp = chrono::system_clock::now();
	chrono::system_clock::duration dtn = tp.time_since_epoch();
	dre.seed(dtn.count());
	uniform_real_distribution<float> uid(-noise_range,noise_range);

	float sample_size = 1000;
	vector<float> filtered,unfiltered,actual;
	filtered.resize(sample_size);
	unfiltered.resize(sample_size);
	actual.resize(sample_size);
	actual[0] = 1000;
	unfiltered[0] = 1000;
	filtered[0] = 1000;
	float a = 1;

	// initialize
	for(float i = 1; i < sample_size; i++){
		actual[i] = exampleFunction(actual[i-1]);
		unfiltered[i] = actual[i] + uid(dre);
	}

	// filter
	float p = 1;
	float g = 1;
	float r = noise_range;

	for(float i = 1; i < sample_size; i++)
		filtered[i] = kalman(unfiltered[i],filtered[i-1],p,g,r);	

	ofstream file("data.csv");
	for(float i = 0; i < sample_size; i++)
		file<<unfiltered[i]<<","<<filtered[i]<<","<<actual[i]<<"\n";
	file.close();

	return 0;
}