kalman.cpp

Kalman filter in c c++ code, the five Kalman equations have been implemented for randomly generated

#include <iostream.h>
#include <conio.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>

void main()
{
	clrscr();
	float tv[25],pv[25],mv[25],pe[25],me[25],temp;  //declaration of true,process,measurement value arrays and process,measurement error arrays
	int samples,range,j=0;    //no. of samples, range of true value,Q and R covariances
	float axest,apest,k,pxest,ppest,q,r;
	cout<<"\n\nNum of samples  ";
	cin>>samples;
	cout<<"Range of true values  ";
	cin>>range;
	cout<<"process error covariance Q  ";
	cin>>q;
	cout<<"measurement error covariance R  ";
	cin>>r;
	float qc=q/100; //correcting Q because it will be divided by 100 while calculating error
	float rc=r/100; //correcting R because it will be divided by 100 while calculating error

	randomize();
	for(int i=0;i<samples;i++) //Generating random true values within specified range
	{
		tv[i]=random(range);
		temp=random(51);
		tv[i]+=temp/100;  //Making true values into FLOAT
		if(random(2)==1)
			tv[i]*=-1;      //Randomly choosing sign
	}


	for(i=0;i<samples;i++)    //Loop to randomly create process error and process value array
	{
		pe[i]=random(q);
		pe[i]/=100;
		if(random(2)==1)
			pe[i]*=-1;
		pv[i]=tv[i]+pe[i];
	}




	for(i=0;i<samples;i++)   //Randomly creating measurement errors and measurement array
	{
		me[i]=(random(r)/100);
		if(random(2)==1)
			me[i]*=-1;
		mv[i]=pv[i]+me[i];
	}


	float xest;
	cout<<"Enter first iteration estimate of true value  ";
	cin>>xest;
	float pest;
	cout<<"Enter first iteration estimate of P(variance)  ";
	cin>>pest;

	float ev[26],epv[26]; //Arrays for final estimated true values and estimated P values(P values only for calculation purpose)

	ev[0]=xest;
	epv[0]=pest;

	for(j=1;j<samples;j++) //Loop to perform Kalman iterations
       {
	axest=ev[j-1];               //the 5 Kalman equations
	apest=epv[j-1]+qc;
	k=apest/(apest+rc);
	pxest=axest+k*(mv[j]-axest);
	ppest=(1-k)*apest;
	ev[j]=pxest;      //Array contains all kalman estimated values
	epv[j]=ppest;     //Array contains all Kalman estimated P covariances used for calculation
	    //To be used for
	   //next iteration
       }
	cout<<"\nMeasured\t"<<"True\t"<<"    Estimated\n";

	for(i=0;i<samples;i++)     //Finally displaying true and estimated values for comparision
	{
		cout<<mv[i]<<"\t\t"<<tv[i]<<"\t\t"<<ev[i]<<"\n";
	}

	getch();

}