kalman滤波器听着很深奥的一个东东，其实一点不难。
我理解的kalman滤波就是通过为对象建立一个特定模型(例如直线匀速运动)，来纠正实际观测到的物体运动轨迹到我们认为的运动模型中。
其特点就是随着时间的推移，物体的实际轨迹会逐渐趋近于真实轨迹(与建立的运动模型越来越吻合)。
一个离散的kalman滤波器可以用2个方程来表示：
时间更新方程：
空间更新方程：
其中几个参数的细微调节会影响整个滤波器的性能。可以通过尝试去调节参数值。
详细介绍参看：
http://www.cs.unc.edu/~welch/kalman/c++实现的代码如下：
kalman.h
#ifndef __KALMAN_H__
#define __KALMAN_H__
#include "Matrix.h"
//discrete kalman filter
class CKalman
{
public:
CKalman();
~CKalman();
public:
/**//**************Init the param********************/
void SetA(CMatrix *pm);
void SetB(CMatrix *pm);
void SetH(CMatrix *pm);
void SetQ(CMatrix *pm);
void SetR(CMatrix *pm);
/**//*****************Input variable******************/
void InputX0(CMatrix *pm);
void InputP0(CMatrix *pm);
void InputUkprev(CMatrix *pm);
void InputZk(CMatrix *pm);
void AssertParam();
public:
//time refresh equation
void RefreshTimeVar();
//status refresh equation
void RefreshStatus();
void OutputResult(double *p);
private:
CMatrix *m_pA; //state plus matrix A
CMatrix *m_pB; //control plus matrix B
CMatrix *m_pPkPriori; //Priori estimation Pk-
CMatrix *m_pPkPosteriori; //Posteriori estimation Pk
CMatrix *m_pPkprev; //Posteriori estimation Pk-1
CMatrix *m_pH;
CMatrix *m_pK; //kalman plus matrix
CMatrix *m_pQ;
CMatrix *m_pR;
CMatrix *m_pXkprev; //Xk-1
CMatrix *m_pXkPriori; //Priori estimation Xk-
CMatrix *m_pXkPosteriori; //Posteriori estimation Xk
CMatrix *m_pUkprev; //control variable;
CMatrix *m_pZk; //observation variable;
bool m_bHasControlVarible;
};
#endif
kalman.cpp:
#include "kalman.h"
#include <assert.h>
CKalman::CKalman()
{
m_pXkprev = NULL;
m_pXkPriori = NULL;
m_pXkPosteriori = NULL;
m_pUkprev = NULL;
m_pZk = NULL;
m_pA = NULL;
m_pB = NULL;
m_pPkPriori = NULL;
m_pPkPosteriori = NULL;
m_pPkprev = NULL;
m_pH = NULL;
m_pK = NULL;
m_pQ = NULL;
m_pR = NULL;
m_bHasControlVarible = false;
}
CKalman::~CKalman()
{
if (NULL != m_pPkPriori)
{
delete m_pPkPriori;
}
if (NULL != m_pPkPosteriori)
{
delete m_pPkPosteriori;
}
if (NULL != m_pXkPriori)
{
delete m_pXkPriori;
}
if (NULL != m_pXkPosteriori)
{
delete m_pXkPosteriori;
}
}
void CKalman::SetA(CMatrix *pm)
{
m_pA = pm;
}
void CKalman::SetB(CMatrix *pm)
{
m_pB = pm;
}
void CKalman::SetH(CMatrix *pm)
{
m_pH = pm;
}
void CKalman::SetQ(CMatrix *pm)
{
m_pQ = pm;
}
void CKalman::SetR(CMatrix *pm)
{
m_pR = pm;
}
void CKalman::AssertParam()
{
assert(NULL != m_pXkprev);
assert(NULL != m_pZk);
assert(NULL != m_pA);
assert(NULL != m_pPkprev);
assert(NULL != m_pH);
assert(NULL != m_pQ);
assert(NULL != m_pR);
if (m_bHasControlVarible)
{
assert(NULL != m_pUkprev);
assert(NULL != m_pB);
}
}
void CKalman::InputX0(CMatrix *pm)
{
m_pXkprev = pm;
long nWidth = m_pXkprev->getWidth();
long nHeight = m_pXkprev->getHeight();
m_pXkPriori = new CMatrix(NULL, nWidth, nHeight);
m_pXkPosteriori = new CMatrix(NULL, nWidth, nHeight);
}
void CKalman::InputP0(CMatrix *pm)
{
m_pPkprev = pm;
long nWidth = m_pPkprev->getWidth();
long nHeight = m_pPkprev->getHeight();
m_pPkPriori = new CMatrix(NULL, nWidth, nHeight);
m_pPkPosteriori = new CMatrix(NULL, nWidth, nHeight);
}
void CKalman::InputUkprev(CMatrix *pm)
{
if (NULL == pm)
{
m_bHasControlVarible = false;
}
else
{
m_bHasControlVarible = true;
m_pUkprev = pm;
}
}
void CKalman::InputZk(CMatrix *pm)
{
m_pZk = pm;
}
void CKalman::RefreshTimeVar()
{
AssertParam();
if (m_bHasControlVarible)
{
*m_pXkPriori = (*m_pA)*(*m_pXkprev) + (*m_pB)*(*m_pUkprev); //1.9
}
else
{
*m_pXkPriori = (*m_pA)*(*m_pXkprev);
}
*m_pPkPriori = (*m_pA)*(*m_pPkprev)*(m_pA->Transpose()) + (*m_pQ); //1.10
}
void CKalman::RefreshStatus()
{
assert(NULL != m_pZk);
CMatrix mPH;
mPH = (*m_pPkPriori) * (m_pH->Transpose());
CMatrix mTmp;
((*m_pH) * mPH + (*m_pR)).GetInverse(mTmp);
long nHeight = mPH.getHeight();
long nWidth = mTmp.getWidth();
CMatrix mK(NULL, nWidth, nHeight);
m_pK = &mK;
*m_pK = mPH * mTmp; //1.11
*m_pXkPosteriori = *m_pXkPriori + (*m_pK) * (*m_pZk - (*m_pH)*(*m_pXkPriori)); //1.12
CMatrix mUnit(m_pK->getHeight());
*m_pPkPosteriori = (mUnit - (*m_pK)*(*m_pH))*(*m_pPkPriori); //1.13
//recursion
m_pPkprev = m_pPkPosteriori;
m_pXkprev = m_pXkPosteriori;
m_pZk = NULL;
}
void CKalman::OutputResult(double *p)
{
m_pXkPosteriori->GetValue(p);
}
其中的matrix实现代码可以参考我以前的文章。
懒了就写到这儿吧
posted on 2011-01-05 15:20
saha 阅读(666)
评论(0) 编辑 收藏 引用