C++下扩展卡尔曼类(EKF)的实现
2018-01-05 11:59
183 查看
最初的卡尔曼滤波用于解决离散系统的滤波问题,然而工程中常遇到的滤波问题是连续系统产生的滤波问题。
卡尔曼滤波器具有如下形式的离散的状态方程:
Xk+1=Ψk+1,kXk+Wk(1)
但这只是一个高度简化的方程,更多的时候我们能获得的是关于连续系统的如下形式的方程:
X˙=f(X)+W(2)
其中f通常是一个非线性方程。
针对上述的非线性方程,扩展卡尔曼滤波的做法如下:
首先,状态一定存在一个轨迹X(t),尽管根据式(2)我们很难具体地解出这个轨迹。假设有一个t0时刻的状态估计值X^,在状态的估计值X^附近把X(t)展开得到
X(t)=X^+∂X∂t∣X=X^(t−t0)+⋯(3)
而
∂X∂t∣X=X^=f(X^)(4)
因此滤波器的时间更新为
Xk+1=Xk+Tf(Xk)(5)
Pk+1=Ψk+1,kPkΨTk+1,k+Qk
而状态转移矩阵Ψk+1,k为
Ψk+1,k=I+T∂X˙∂X∣X=X^(6)
测量方程随可能同样为非线性
Z=h(X)+V
但它可以直接线性化
Zk=h(X^)+∂Z∂X∣X=X^(Xk−X^)+⋯
于是H阵为H=∂Z∂X∣X=X^
接下来只剩下测量更新,按照卡尔曼滤波的那一套来执行就行了。
另外,上述在式(3)只保留了一阶项,若嫌精度低可保留二阶项,相应的,状态转移矩阵也要保留二阶项。
一般的,卡尔曼滤波的Q阵要比R阵更难准确获得,因此建议先确定R阵,之后经验性的给定Q阵,观察滤波效果以决定对Q阵的调整。
在C++下将上述过程编写为类。考虑通用性,对所有关于状态X的方程都抽象为如下形式的函数
mat是矩阵类,第一个参数time是时刻,第二个参数X是状态,第三个参数para是方程中需要用到的参数。
以四元数微分方程为例:
Q˙=12[ω×]Q
则X=Q,para=ω,函数返回Q˙
头文件kalman.h,其中矩阵类mat和时间戳类time_stamp请自行实现:
cpp文件kalman.cpp:
卡尔曼滤波器具有如下形式的离散的状态方程:
Xk+1=Ψk+1,kXk+Wk(1)
但这只是一个高度简化的方程,更多的时候我们能获得的是关于连续系统的如下形式的方程:
X˙=f(X)+W(2)
其中f通常是一个非线性方程。
针对上述的非线性方程,扩展卡尔曼滤波的做法如下:
首先,状态一定存在一个轨迹X(t),尽管根据式(2)我们很难具体地解出这个轨迹。假设有一个t0时刻的状态估计值X^,在状态的估计值X^附近把X(t)展开得到
X(t)=X^+∂X∂t∣X=X^(t−t0)+⋯(3)
而
∂X∂t∣X=X^=f(X^)(4)
因此滤波器的时间更新为
Xk+1=Xk+Tf(Xk)(5)
Pk+1=Ψk+1,kPkΨTk+1,k+Qk
而状态转移矩阵Ψk+1,k为
Ψk+1,k=I+T∂X˙∂X∣X=X^(6)
测量方程随可能同样为非线性
Z=h(X)+V
但它可以直接线性化
Zk=h(X^)+∂Z∂X∣X=X^(Xk−X^)+⋯
于是H阵为H=∂Z∂X∣X=X^
接下来只剩下测量更新,按照卡尔曼滤波的那一套来执行就行了。
另外,上述在式(3)只保留了一阶项,若嫌精度低可保留二阶项,相应的,状态转移矩阵也要保留二阶项。
一般的,卡尔曼滤波的Q阵要比R阵更难准确获得,因此建议先确定R阵,之后经验性的给定Q阵,观察滤波效果以决定对Q阵的调整。
在C++下将上述过程编写为类。考虑通用性,对所有关于状态X的方程都抽象为如下形式的函数
mat equation(time_stamp& time,mat& X,mat& para);
mat是矩阵类,第一个参数time是时刻,第二个参数X是状态,第三个参数para是方程中需要用到的参数。
以四元数微分方程为例:
Q˙=12[ω×]Q
则X=Q,para=ω,函数返回Q˙
头文件kalman.h,其中矩阵类mat和时间戳类time_stamp请自行实现:
#ifndef KALMAN_H #define KALMAN_H //时间更新发生在测量完成之后,即测量完成时刻为当前滤波时刻 //因此时间更新指从上一个滤波时刻更新到当前滤波时刻 typedef mat (*pEquation_of_X)(time_stamp& time,mat& X,mat& para); class EKF { public: EKF(); ~EKF(); //指向状态微分方程 pEquation_of_X pState_differential_equation; //指向状态微分方程的雅可比矩阵的计算 pEquation_of_X pJacob_of_state_DE; //指向测量方程的雅可比矩阵即H阵的计算 pEquation_of_X pJacob_of_measure_equation; //指向测量方程 pEquation_of_X pMeasure_equation; mat X;//状态 mat P;//状态协方差 mat Z;//测量量 mat Q;//Q阵 mat R;//R阵 mat H;//H阵 mat innovation;//新息 mat gain;//增益阵 mat para_of_time_update;//时间更新的参数矩阵 mat para_of_measure_update;//测量更新的参数矩阵 time_stamp time;//指示滤波器的时间戳 void time_update(void);//时间更新 void measure_update(void);//测量更新 }; #endif
cpp文件kalman.cpp:
#include <iostream> #include "kalman.h" EKF::EKF() { pState_differential_equation=NULL; pJacob_of_state_DE=NULL; pJacob_of_measure_equation=NULL; pMeasure_equation=NULL; } EKF::~EKF() {} void EKF::time_update(void) { if(pState_differential_equation!=NULL&&pJacob_of_state_DE!=NULL) { double detT=time.current_sec-time.last_sec; X=X+detT*pState_differential_equation(time,X,para_of_time_update); mat STM=detT*pJacob_of_state_DE(time,X,para_of_measure_update); for(int i=0;i<STM.rows();i++) { STM[i][i]=STM[i][i]+1; } P=STM*P*STM.trans()+Q; } else { std::cout<<"state differential equation is NULL or Jacob matrix of SDE is NULL!"<<std::endl; } } void EKF::measure_update(void) { if(pMeasure_equation!=NULL&&pJacob_of_measure_equation!=NULL) { innovation=Z-pMeasure_equation(time,X,para_of_measure_update); H=pJacob_of_measure_equation(time,X,para_of_measure_update); gain=P*H.trans()*inv(H*P*H.trans()+R); X=X+gain*innovation; P=(diag(X.rows(),1)-gain*H)*P; } else { std::cout<<"measure equation is NULL or jacob of ME is NULL!"<<std::endl; } }
相关文章推荐
- C++ FastDelegate 扩展,实现与.net类似的事件处理功能
- 生日悖论扩展引申1-c++代码实现及运行实例结果
- 生日悖论扩展引申2-c++代码实现及运行实例结果
- 利用C/C++扩展php语言实现 Usher_GetHostIP无参函数
- 生日悖论扩展引申1-c++代码实现及运行实例结果
- 生日悖论扩展引申1-c++代码实现及运行实例结果
- Node.js C++扩展实现
- 生日悖论扩展引申1-c++代码实现及运行实例结果
- C++实现PHP扩展
- 编译器对C++ 11变参模板(Variadic Template)的函数包扩展实现的差异
- 利用MFC实现浏览器的定制与扩展(JavaScript与C++交互)
- 利用C/C++扩展php语言实现 Usher_GetHostIP无参函数
- 生日悖论扩展引申1-c++代码实现及运行实例结果
- 各种实现下C++的扩展名。
- C++实现 八皇后问题及其扩展N皇后问题(经典回溯算法)
- 生日悖论扩展引申1-c++代码实现及运行实例结果
- 探究C++中string类的实现原理以及扩展使用
- 生日悖论扩展引申1-c++代码实现及运行实例结果
- 生日悖论扩展引申2-c++代码实现及运行实例结果
- 使用SWIG实现C++扩展Python