资源简介
一个基于MFC的Kalman滤波比较程序,简化版本。包含EKF,CKF,UKF,ICKF,FCKF等Kalman滤波算法,对学习和了解Kalman滤波算法有很大帮助,对研究Kalman滤波、数据融合、图像处理、雷达算法等领域的研究生、学者也有一定的借鉴意义。
由于该程序是一个简化的版本,可能有错,请勿直接拿到项目中直接使用!不得用于任何商业用途,使用本程序时,说明出处即可自由使用!
代码片段和文件信息
#pragma once
#include “StdAfx.h“
#include “EKF.h“
//#include “SettingDlg.h“
#include
//#include “RealTrack.h“
EKF::EKF(void)
{
//memset(X_EKF 0 sizeof(X_EKF));
}
EKF::~EKF(void)
{
}
void EKF::EKFDelete(CPtrArray &X)
{
#pragma omp parallel for
for (int i = 0; i < X.GetSize(); ++i)
{
delete (StoreData *)X.GetAt(i);
}
X.RemoveAll();
}
/*EKF::EKF(double x1 double x2 double x3 double x4)
{
memset(X_EKF 0 sizeof(X_EKF));
this->X_EKF[0][0] = x1;
this->X_EKF[1][0] = x2;
this->X_EKF[2][0] = x3;
this->X_EKF[3][0] = x4;
}*/
void EKF::EKFfiltering(double *z UINT m_length)
{
//CMatrix fai(4 4);
//fai(1 1) = fai(1 2) = fai(2 2) = fai(3 3) = fai(3 4) = fai(4 4) = 1;
//CMatrix gama(4 2);
//gama(1 1) = gama(3 2) = 0.5;
//gama(2 1) = gama(4 2) = 1;
CMatrix Pplus(4 4);
Pplus = glb.Pplus;
CMatrix xhat(4 1);
xhat = glb.xhat;
StoreData *ekf = new StoreData(xhat(1 1) xhat(2 1) xhat(3 1) xhat(4 1));
X.Add(ekf);
//CMatrix I(4 4);
//I(1 1) = I(2 2) = I(3 3) = I(4 4) = 1;
//CMatrix Im(2 2);
//Im(1 1) = Im(2 2) = Qn;
CMatrix H(1 dim);
CMatrix K(dim 1);
//CMatrix tem(1 1);
//CMatrix CMte(dim 1);
CMatrix Znum(1 1);
int num = 1;//第0个未知存放的是占位符0,并不是真正的测量值,因此应当从下标1开始取测量值
for (UINT count = TimeInterval; count <= m_length; count += TimeInterval)
{
Pplus = glb.fai * Pplus * (~glb.fai) + glb.gama * glb.Im * (~glb.gama);
H(1 1) = -xhat(3 1) / ((1 + pow(xhat(3 1) / xhat(1 1) 2)) * pow(xhat(1 1) 2));
H(1 2) = H(1 4) = 0;
H(1 3) = 1 / ((1 + pow(xhat(3 1) / xhat(1 1) 2)) * xhat(1 1));
//CMatrix tem(1 1);
Znum(1 1) = Rn;//For saving memory
Znum = H * Pplus * (~H) + Znum;
K = ((double)1 / Znum(1 1)) *Pplus * (~H);
xhat = glb.fai * xhat;
xhat = xhat + (z[num] - atan(xhat(3 1) / xhat(1 1))) * K;
Pplus = (glb.I - K * H) * Pplus;
++num;
StoreData *ekf = new StoreData(xhat(1 1) xhat(2 1) xhat(3 1) xhat(4 1));
X.Add(ekf);
}
}
属性 大小 日期 时间 名称
----------- --------- ---------- ----- ----
文件 924 2013-06-06 19:07 Nonlinear_Filtering.sln
文件 15469286 2014-06-21 19:47 Debug\help.pdf
文件 9637888 2015-02-13 22:13 Debug\Nonlinear_Filtering.exe
文件 2119 2013-06-14 21:52 Nonlinear_Filtering\EKF.cpp
文件 440 2013-06-14 15:07 Nonlinear_Filtering\EKF.h
文件 5169 2014-06-21 19:34 Nonlinear_Filtering\FifthCKF.cpp
文件 501 2013-06-14 15:12 Nonlinear_Filtering\FifthCKF.h
文件 2932 2014-06-21 19:35 Nonlinear_Filtering\Globals.cpp
文件 1408 2013-06-15 01:46 Nonlinear_Filtering\Globals.h
文件 93055 2015-02-13 22:10 Nonlinear_Filtering\Graph.cpp
文件 4964 2015-02-13 22:06 Nonlinear_Filtering\Graph.h
文件 918 2015-02-13 22:10 Nonlinear_Filtering\GraphDataColor.cpp
文件 1265 2015-02-13 22:06 Nonlinear_Filtering\GraphDataColor.h
文件 981 2015-02-13 22:10 Nonlinear_Filtering\GraphDataSet.cpp
文件 1245 2015-02-13 22:06 Nonlinear_Filtering\GraphDataSet.h
文件 2404 2015-02-13 22:06 Nonlinear_Filtering\GraphLegend.cpp
文件 1341 2015-02-13 22:07 Nonlinear_Filtering\GraphLegend.h
文件 945 2013-06-08 12:40 Nonlinear_Filtering\GraphLegendSet.cpp
文件 1301 2015-02-13 22:07 Nonlinear_Filtering\GraphLegendSet.h
文件 2148 2015-02-13 22:10 Nonlinear_Filtering\GraphSeries.cpp
文件 1466 2015-02-13 22:07 Nonlinear_Filtering\GraphSeries.h
文件 3866 2014-06-21 19:34 Nonlinear_Filtering\ICKF.cpp
文件 467 2013-06-14 15:07 Nonlinear_Filtering\ICKF.h
文件 10688 2013-06-16 21:03 Nonlinear_Filtering\MainFrm.cpp
文件 1237 2013-06-17 23:24 Nonlinear_Filtering\MainFrm.h
文件 8587 2013-12-07 18:38 Nonlinear_Filtering\Matrix.cpp
文件 2071 2013-12-07 18:39 Nonlinear_Filtering\Matrix.h
文件 133952 2013-06-18 23:46 Nonlinear_Filtering\Nonlinear_Filtering.aps
文件 5250 2013-06-09 09:52 Nonlinear_Filtering\Nonlinear_Filtering.cpp
文件 741 2013-06-06 19:07 Nonlinear_Filtering\Nonlinear_Filtering.h
............此处省略35个文件信息
- 上一篇:MFC vc++ 银行业务模拟系统
- 下一篇:c++课程设计之车票管理系统
评论
共有 条评论