• 大小: 3.41MB
    文件类型: .rar
    金币: 1
    下载: 0 次
    发布日期: 2023-11-06
  • 语言: C/C++
  • 标签: Kalman  filtering  EKF  CKF  UKF  

资源简介

一个基于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个文件信息

评论

共有 条评论