kalman濾波器聽(tīng)著很深?yuàn)W的一個(gè)東東，其實(shí)一點(diǎn)不難。
我理解的kalman濾波就是通過(guò)為對(duì)象建立一個(gè)特定模型(例如直線勻速運(yùn)動(dòng))，來(lái)糾正實(shí)際觀測(cè)到的物體運(yùn)動(dòng)軌跡到我們認(rèn)為的運(yùn)動(dòng)模型中。
其特點(diǎn)就是隨著時(shí)間的推移，物體的實(shí)際軌跡會(huì)逐漸趨近于真實(shí)軌跡(與建立的運(yùn)動(dòng)模型越來(lái)越吻合)。
一個(gè)離散的kalman濾波器可以用2個(gè)方程來(lái)表示：
時(shí)間更新方程：
空間更新方程：
其中幾個(gè)參數(shù)的細(xì)微調(diào)節(jié)會(huì)影響整個(gè)濾波器的性能。可以通過(guò)嘗試去調(diào)節(jié)參數(shù)值。
詳細(xì)介紹參看：
http://www.cs.unc.edu/~welch/kalman/c++實(shí)現(xiàn)的代碼如下：
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實(shí)現(xiàn)代碼可以參考我以前的文章。
懶了就寫到這兒吧
posted on 2011-01-05 15:20
saha 閱讀(672)
評(píng)論(0) 編輯 收藏 引用