自适应卡尔曼滤波算法

自适应卡尔曼滤波算法，适用于手机陀螺仪数据降噪，预测

// 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个文件信息