資源簡介
自適應卡爾曼濾波算法,適用于手機陀螺儀數據降噪,預測

代碼片段和文件信息
//?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.em
?????文件????????728??2013-04-08?18:48??adapkalman\adapkalman\Debug\adapkalman.exe.em
?????文件????????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個文件信息
- 上一篇:OD點擊寄存器變色OD
- 下一篇:大話移動通信網絡規劃 高清掃描版
評論
共有 條評論