//?kalman.cpp?:?Defines?the?entry?point?for?the?console?application.
//

#include?“stdafx.h“
#include?
#include?
#include?“opencv2/opencv.hpp“
#ifdef?_DEBUG
#pragma?comment（?lib?“opencv_core243d.lib“?）
#pragma?comment（?lib?“opencv_highgui243d.lib“?）
#pragma?comment（?lib?“opencv_imgproc243d.lib“?）
#pragma?comment（?lib?“opencv_video243d.lib“）
#else
#pragma?comment（?lib?“opencv_core243.lib“?）
#pragma?comment（?lib?“opencv_highgui243.lib“?）
#pragma?comment（?lib?“opencv_imgproc243.lib“?）
#endif

#define?SPD_FLOAT?double
#define?SPD_FLOAT_CV?CV_64F
#define?SPD_FLOAT3?double3

struct?float3
{
	float?xyz;
};
struct?double3
{
	double?xyz;
};

class?kalman
{
public:
	kalman（int?n?int?m?int?M?bool?control=false）;
	~kalman（）
	{

	}
	void?process（SPD_FLOAT*?z?SPD_FLOAT*?x）;
	/*?matF:狀態轉移矩陣
	matB:控制矩陣
	matH:觀察矩陣
	matQ:過程噪聲的協方差矩陣
	matR:觀測噪聲的協方差矩陣
	matP0:前幀估計誤差的協方差矩陣
	matP1:后幀估計誤差的協方差矩陣
	vecX0
	vecX1
	vecU
	vecZ?*/
	std::vector??filterList;??//并行濾波器的個數及每個濾波器采用的新息序列滑動窗口的寬度。
	int??M;???//并行濾波器個數；
	CvMat?*matF*matB*matH*matQ*matR*matP0*matP1?*matKT;
	std::queue?*IV*CIV;???//IV:新息狀態，CIV：新息狀態的方差
	CvMat?*vecX0*vecX1*vecU*vecZ;
	CvMat?*tmp[4];
	bool?bControl;
};

kalman::kalman（int?n?int?mint?M?bool?control）
{
	matF[0]?=?cvCreateMat（nnSPD_FLOAT_CV）;
	matF[1]?=?cvCreateMat（nnSPD_FLOAT_CV）;
	matB?=?cvCreateMat（nnSPD_FLOAT_CV）;
	matH?=?cvCreateMat（mnSPD_FLOAT_CV）;
	matQ?=?cvCreateMat（nnSPD_FLOAT_CV）;
	matR?=?cvCreateMat（mmSPD_FLOAT_CV）;
	matP0?=?cvCreateMat（nnSPD_FLOAT_CV）;
	matP1?=?cvCreateMat（nnSPD_FLOAT_CV）;
	matKT?=?cvCreateMat（mnSPD_FLOAT_CV）;

	vecX0?=?cvCreateMat（n1SPD_FLOAT_CV）;
	vecX1?=?cvCreateMat（n1SPD_FLOAT_CV）;
	vecU?=?cvCreateMat（n1SPD_FLOAT_CV）;
	vecZ?=?cvCreateMat（m1SPD_FLOAT_CV）;

	tmp[0]?=?cvCreateMat（nnSPD_FLOAT_CV）;
	tmp[1]?=?cvCreateMat（mnSPD_FLOAT_CV）;
	tmp[2]?=?cvCreateMat（mmSPD_FLOAT_CV）;
	tmp[3]?=?cvCreateMat（m1SPD_FLOAT_CV）;

	IV=NULL;
	CIV=NULL;

	filterList.push_back（1）;
	filterList.push_back（2）;??//默認采用兩個并行濾波器，滑動窗口分別為12

	bControl?=?control;
}

void?kalman::process（SPD_FLOAT*?z?SPD_FLOAT*?x）
{
	if（!z?||?!x）
		return;

	memcpy（vecZ->data.ptr?z?vecZ->cols*vecZ->rows*sizeof（SPD_FLOAT））;

	//1.?predict
	if（bControl）
	{
	}

	cvMatMul（matF?vecX0?vecX1）;
	cvMatMul（matF?matP0?tmp[0]）;
	cvGEMM（tmp[0]?matF?1.0?matQ?1.0?matP1?CV_GEMM_B_T）;
	

	//2.?correct
	cvMatMul（matH?matP1?tmp[1]）;
	cvGEMM（tmp[1]?matH?1.0?matR?1.0?tmp[2]?CV_GEMM_B_T）;
	cvSolve（tmp[2]?tmp[1]?matKT）;


	cvGEMM（matH?vecX1?-1.0?vecZ?1.0?tmp[3]）;
	cvGEMM（matKT?tmp[3]?1.0?vecX1?1.0?vecX1?CV_GEMM_A_T）;
	cvGEMM（matKT?tmp[1]?-1.0?matP1?1.0?matP1?CV_GEMM_A_T）;


	memcpy（x?vecX1->data.ptr?vecZ->rows*vecZ->cols*sizeof（SPD_FLOAT））;
	//3.?update?
	cvCopy（vecX1?vecX0）;
	cvCopy（matP1?matP0）;
}



int?_tmain（int?argc?_TCHAR*?argv[]）
{
	//楨

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????5122??2013-04-09?15:26??adapkalman\adapkalman\adapkalman.cpp

?????文件???????4615??2013-04-08?18:48??adapkalman\adapkalman\adapkalman.vcproj

?????文件???????1407??2013-04-15?19:50??adapkalman\adapkalman\adapkalman.vcproj.SUPERD.Dean.Li.user

?????文件????????663??2013-04-08?18:48??adapkalman\adapkalman\Debug\adapkalman.exe.embed.manifest

?????文件????????728??2013-04-08?18:48??adapkalman\adapkalman\Debug\adapkalman.exe.embed.manifest.res

?????文件????????621??2013-04-09?15:24??adapkalman\adapkalman\Debug\adapkalman.exe.intermediate.manifest

?????文件????3211264??2013-04-08?18:47??adapkalman\adapkalman\Debug\adapkalman.pch

?????文件???????6030??2013-04-09?15:26??adapkalman\adapkalman\Debug\BuildLog.htm

?????文件?????????67??2013-04-09?15:24??adapkalman\adapkalman\Debug\mt.dep

?????文件??????12641??2013-04-08?18:47??adapkalman\adapkalman\Debug\stdafx.obj

?????文件?????478208??2013-04-09?15:26??adapkalman\adapkalman\Debug\vc90.idb

?????文件????1118208??2013-04-09?15:26??adapkalman\adapkalman\Debug\vc90.pdb

?????文件???????1320??2013-04-08?18:30??adapkalman\adapkalman\ReadMe.txt

?????文件????????297??2013-04-08?18:30??adapkalman\adapkalman\stdafx.cpp

?????文件????????320??2013-04-08?18:30??adapkalman\adapkalman\stdafx.h

?????文件????????765??2013-04-08?18:30??adapkalman\adapkalman\targetver.h

?????文件????4803584??2013-04-15?19:50??adapkalman\adapkalman.ncb

?????文件????????896??2013-04-08?18:30??adapkalman\adapkalman.sln

????..A..H.?????12800??2013-04-15?19:50??adapkalman\adapkalman.suo

?????文件???????3439??2012-10-18?06:56??adapkalman\cv243\opencv\cv.h

?????文件???????2411??2012-07-28?06:59??adapkalman\cv243\opencv\cv.hpp

?????文件???????2850??2012-07-28?06:59??adapkalman\cv243\opencv\cvaux.h

?????文件???????2346??2012-07-28?06:59??adapkalman\cv243\opencv\cvaux.hpp

?????文件???????2184??2012-10-18?06:56??adapkalman\cv243\opencv\cvwimage.h

?????文件???????2465??2012-10-18?06:56??adapkalman\cv243\opencv\cxcore.h

?????文件???????2423??2012-07-28?06:59??adapkalman\cv243\opencv\cxcore.hpp

?????文件???????2266??2012-07-28?06:59??adapkalman\cv243\opencv\cxeigen.hpp

?????文件????????110??2012-07-28?06:59??adapkalman\cv243\opencv\cxmisc.h

?????文件???????2306??2012-07-28?06:59??adapkalman\cv243\opencv\highgui.h

?????文件???????2189??2012-07-28?06:59??adapkalman\cv243\opencv\ml.h

............此處省略187個文件信息