main.cpp.svn-base

卡尔曼滤波的实现

#include <stdio.h>
#include "operator.h"

void main()
{
	int i,j;
	Matrix* measure=initMatrix(3,1);
	Matrix* temp;
	Kalman* kal=initKalman(3,3,3);
	kal->lastState->data[0]=25;
	kal->lastState->data[1]=36;
	kal->lastState->data[2]=6;
	measure->data[0]=36;
	measure->data[1]=36;
	measure->data[2]=6;
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			if(i==j)
			{
		    kal->lastP->data[i*3+j]=3;
			}
		}
	}
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			if(i==j)
			{
				kal->A->data[i*3+j]=1;
			}
		}
	}
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			if(i==j)

				kal->H->data[i*3+j]=1;
		}
	}
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			if(i==j)
			{
				kal->Q->data[i*3+j]=0.03;
			}
		}
	}
	for(i=0;i<3;i++)
	{
		for(j=0;j<3;j++)
		{
			if(i==j)
			{
				kal->R->data[i*3+j]=1.5;
			}
		}
	}
/*	kal->H->data[0]=1;
    kal->H->data[1]=1;
	kal->H->data[2]=0;
    kal->H->data[3]=1;
	kal->H->data[4]=0;
	kal->H->data[5]=1;
	kal->H->data[6]=0;
	kal->H->data[7]=1;
	kal->H->data[8]=1;*/
	
	//temp=getInverseMatrix(kal->lastP);
	KalmanFilter(kal,measure);
	for(i=0;i<3;i++)
		printf("%f\n",kal->modifyState->data[i]);
}