rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim > 模板类 参考

扩展卡尔曼滤波器 更多...

#include <rmvl/algorithm/kalman.hpp>

类 rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim > 继承关系图:

rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim > 的协作图:

Public 成员函数
	ExtendedKalmanFilter ()
	构造新的 ExtendedKalmanFilter 对象
void	setJa (const cv::Matx< Tp, StateDim, StateDim > &state_jac)
	设置状态方程雅可比矩阵 \(J_A\)
void	setJh (const cv::Matx< Tp, MeasureDim, StateDim > &observe_jac)
	设置观测方程雅可比矩阵 \(J_H\)
void	setFa (const std::function< cv::Matx< Tp, StateDim, 1 >(const cv::Matx< Tp, StateDim, 1 > &)> &state_func)
	设置非线性的离散状态方程 \(\pmb f_A(\pmb x)\)
void	setFh (const std::function< cv::Matx< Tp, MeasureDim, 1 >(const cv::Matx< Tp, StateDim, 1 > &)> &observe_func)
	设置非线性的离散观测方程 \(\pmb f_H(\pmb x)\)
void	setW (const cv::Matx< Tp, StateDim, StateDim > &process_jac)
	设置过程噪声协方差雅可比矩阵 \(W\)
void	setV (const cv::Matx< Tp, MeasureDim, MeasureDim > &measure_jac)
	设置测量噪声协方差雅可比矩阵 \(V\)
auto	predict ()
	扩展卡尔曼滤波的预测部分，包括状态量的先验估计和误差协方差的先验估计
auto	correct (const cv::Matx< Tp, MeasureDim, 1 > &zk)
	扩展卡尔曼滤波的校正部分，包含卡尔曼增益的计算、状态量的后验估计和误差协方差的后验估计
Public 成员函数 继承自 rm::KalmanFilterStaticDatas< Tp, StateDim, MeasureDim >
	KalmanFilterStaticDatas ()
	构造新的卡尔曼滤波静态数据
void	init (const cv::Matx< Tp, StateDim, 1 > &x0, Tp error)
	初始化状态以及对应的误差协方差矩阵（常数对角矩阵）
void	init (const cv::Matx< Tp, StateDim, 1 > &x0, const cv::Matx< Tp, StateDim, 1 > &error)
	初始化状态以及对应的误差协方差矩阵（对角矩阵）
void	setR (const cv::Matx< Tp, MeasureDim, MeasureDim > &measure_err)
	设置测量噪声协方差矩阵 \(R\)
void	setQ (const cv::Matx< Tp, StateDim, StateDim > &process_err)
	设置过程噪声协方差矩阵 \(Q\)
void	setP (const cv::Matx< Tp, StateDim, StateDim > &state_err)
	设置误差协方差矩阵 \(P\)

额外继承的成员函数
Protected 属性 继承自 rm::KalmanFilterStaticDatas< Tp, StateDim, MeasureDim >
cv::Matx< Tp, StateDim, 1 >	x
	状态的后验估计 \(\hat{\pmb x}\)
cv::Matx< Tp, StateDim, 1 >	x_
	状态的先验估计 \(\hat{\pmb x}^-\)
cv::Matx< Tp, MeasureDim, 1 >	z
	观测向量 \(\pmb z\)
cv::Matx< Tp, StateDim, StateDim >	Q
	过程噪声协方差矩阵 \(Q\)
cv::Matx< Tp, MeasureDim, MeasureDim >	R
	测量噪声协方差矩阵 \(R\)
cv::Matx< Tp, StateDim, StateDim >	P
	后验误差协方差矩阵 \(P\)
cv::Matx< Tp, StateDim, StateDim >	P_
	先验误差协方差矩阵 \(P^-\)
cv::Matx< Tp, StateDim, StateDim >	I
	单位矩阵 \(I\)
cv::Matx< Tp, StateDim, MeasureDim >	K
	卡尔曼增益 \(K\)

详细描述

template<typename Tp, unsigned StateDim, unsigned MeasureDim>
class rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >

扩展卡尔曼滤波器

模板参数

Tp	数据类型
StateDim	状态量个数
MeasureDim	观测量个数

构造及析构函数说明

ExtendedKalmanFilter()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >::ExtendedKalmanFilter ( )

inline

构造新的 ExtendedKalmanFilter 对象

成员函数说明

correct()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

auto rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >::correct ( const cv::Matx< Tp, MeasureDim, 1 > & zk )

inline

扩展卡尔曼滤波的校正部分，包含卡尔曼增益的计算、状态量的后验估计和误差协方差的后验估计

公式如下

\[\begin{align}K_k&=P_k^-J_H^T\left(J_HP_k^-J_H^T+VRV^T\right)^{-1}\\\hat{\pmb x} &=\hat{\pmb x}_k^-+K_k\left[\pmb z_k-\pmb f_H(\hat{\pmb x}_k^-)\right]\\P_k&=\left(I-K_kJ_H \right)P_k^-\end{align}\]

参数

[in]

zk

观测量

返回

后验状态估计

predict()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

auto rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >::predict ( )

inline

扩展卡尔曼滤波的预测部分，包括状态量的先验估计和误差协方差的先验估计

公式如下

\[\begin{align}\hat{\pmb x_k}^-&=\pmb f(\hat{\pmb x}_{k-1})\\ P_k^-&=J_AP_{k-1}J_A^T+WQW^T\end{align}\]

返回

先验状态估计

setFa()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

void rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >::setFa ( const std::function< cv::Matx< Tp, StateDim, 1 >(const cv::Matx< Tp, StateDim, 1 > &)> & state_func )

inline

设置非线性的离散状态方程 \(\pmb f_A(\pmb x)\)

参数

[in]

state_func

状态方程

setFh()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

void rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >::setFh ( const std::function< cv::Matx< Tp, MeasureDim, 1 >(const cv::Matx< Tp, StateDim, 1 > &)> & observe_func )

inline

设置非线性的离散观测方程 \(\pmb f_H(\pmb x)\)

参数

[in]

observe_func

观测方程

setJa()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

void rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >::setJa ( const cv::Matx< Tp, StateDim, StateDim > & state_jac )

inline

设置状态方程雅可比矩阵 \(J_A\)

参数

[in]

state_jac

状态方程雅可比矩阵

setJh()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

void rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >::setJh ( const cv::Matx< Tp, MeasureDim, StateDim > & observe_jac )

inline

设置观测方程雅可比矩阵 \(J_H\)

参数

[in]

observe_jac

观测方程雅可比矩阵

setV()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

void rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >::setV ( const cv::Matx< Tp, MeasureDim, MeasureDim > & measure_jac )

inline

设置测量噪声协方差雅可比矩阵 \(V\)

参见

1.2 观测方程线性化

参数

[in]

measure_jac

测量噪声协方差雅可比矩阵

setW()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

void rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >::setW ( const cv::Matx< Tp, StateDim, StateDim > & process_jac )

inline

设置过程噪声协方差雅可比矩阵 \(W\)

参见

1.1 状态方程线性化

参数

[in]

process_jac

过程噪声协方差雅可比矩阵

---

该类的文档由以下文件生成:

• rmvl/algorithm/kalman.hpp