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

#include <rmvl/algorithm/kalman.hpp>

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

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

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

Protected 类型
template<int OutputDim>
using	StateFunction = std::function<cv::Matx<FloatTp, OutputDim, 1>(const cv::Matx<FloatTp, StateDim, 1> &)>

静态 Protected 成员函数
template<int OutputDim>
static cv::Matx< FloatTp, OutputDim, StateDim >	calcJ (const StateFunction< OutputDim > &func, const cv::Matx< FloatTp, StateDim, 1 > &x)
	根据非线性函数计算当前状态处的雅可比矩阵

Protected 属性
cv::Matx< FloatTp, StateDim, StateDim >	Ja
	状态方程雅可比矩阵 \(J_A\)
cv::Matx< FloatTp, StateDim, StateDim >	Jat
	状态方程雅可比矩阵的转置 \(J_A^T\)
cv::Matx< FloatTp, MeasureDim, StateDim >	Jh
	观测方程雅可比矩阵 \(J_H\)
cv::Matx< FloatTp, StateDim, MeasureDim >	Jht
	观测方程雅可比矩阵的转置 \(J_H^T\)
cv::Matx< FloatTp, StateDim, StateDim >	W
	过程噪声协方差雅可比矩阵 \(W\)
cv::Matx< FloatTp, StateDim, StateDim >	Wt
	过程噪声协方差雅可比矩阵的转置 \(W^T\)
cv::Matx< FloatTp, MeasureDim, MeasureDim >	V
	测量噪声协方差雅可比矩阵 \(V\)
cv::Matx< FloatTp, MeasureDim, MeasureDim >	Vt
	测量噪声协方差雅可比矩阵的转置 \(V^T\)
std::function< cv::Matx< FloatTp, StateDim, 1 >(const cv::Matx< FloatTp, StateDim, 1 > &)>	Fa
	非线性的离散状态方程
std::function< cv::Matx< FloatTp, MeasureDim, 1 >(const cv::Matx< FloatTp, StateDim, 1 > &)>	Fh
	非线性的离散观测方程
Protected 属性继承自 rm::KalmanFilterStaticDatas< FloatTp, StateDim, MeasureDim >
cv::Matx< FloatTp, StateDim, 1 >	x
	状态的后验估计 \(\hat{\boldsymbol x}\)
cv::Matx< FloatTp, StateDim, 1 >	x_
	状态的先验估计 \(\hat{\boldsymbol x}^-\)
cv::Matx< FloatTp, MeasureDim, 1 >	z
	观测向量 \(\boldsymbol z\)
cv::Matx< FloatTp, StateDim, StateDim >	Q
	过程噪声协方差矩阵 \(Q\)
cv::Matx< FloatTp, MeasureDim, MeasureDim >	R
	测量噪声协方差矩阵 \(R\)
cv::Matx< FloatTp, StateDim, StateDim >	P
	后验误差协方差矩阵 \(P\)
cv::Matx< FloatTp, StateDim, StateDim >	P_
	先验误差协方差矩阵 \(P^-\)
cv::Matx< FloatTp, StateDim, StateDim >	I
	单位矩阵 \(I\)
cv::Matx< FloatTp, StateDim, MeasureDim >	K
	卡尔曼增益 \(K\)

详细描述

template<typename FloatTp, int StateDim, int MeasureDim>
class rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >

扩展卡尔曼滤波器

模板参数

FloatTp	浮点数据类型
StateDim	状态量个数
MeasureDim	观测量个数

注解: 提供了形如 rm::EKF31f 、 rm::EKF31d 之类的类型别名，以头文件定义为准

成员类型定义说明

◆ StateFunction

template<typename FloatTp, int StateDim, int MeasureDim>

template<int OutputDim>

using rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::StateFunction = std::function<cv::Matx<FloatTp, OutputDim, 1>(const cv::Matx<FloatTp, StateDim, 1> &)>

protected

构造及析构函数说明

◆ ExtendedKalmanFilter()

template<typename FloatTp, int StateDim, int MeasureDim>

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

inline

构造新的 ExtendedKalmanFilter 对象

成员函数说明

◆ calcJ()

template<typename FloatTp, int StateDim, int MeasureDim>

template<int OutputDim>

cv::Matx< FloatTp, OutputDim, StateDim > rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::calcJ	(	const StateFunction< OutputDim > &	func,
		const cv::Matx< FloatTp, StateDim, 1 > &	x )

inlinestaticprotected

根据非线性函数计算当前状态处的雅可比矩阵

参数

[in]	func	非线性状态方程或观测方程
[in]	x	线性化点

返回: 当前状态处的雅可比矩阵

◆ correct()

template<typename FloatTp, int StateDim, int MeasureDim>

auto rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::correct ( const cv::Matx< FloatTp, 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{\boldsymbol x} &=\hat{\boldsymbol x}_k^-+K_k\left[\boldsymbol z_k-\boldsymbol f_H(\hat{\boldsymbol x}_k^-)\right]\\P_k&=\left(I-K_kJ_H \right)P_k^-\end{align}\]

参数

[in] zk 观测量

返回: 后验状态估计

◆ predict()

template<typename FloatTp, int StateDim, int MeasureDim>

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

inline

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

公式如下

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

返回: 先验状态估计

◆ setFa()

template<typename FloatTp, int StateDim, int MeasureDim>

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

inline

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

参数

[in] state_func 状态方程

◆ setFh()

template<typename FloatTp, int StateDim, int MeasureDim>

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

inline

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

参数

[in] observe_func 观测方程

◆ setJa()

template<typename FloatTp, int StateDim, int MeasureDim>

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

inline

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

弃用: 雅可比矩阵会在 predict() 中根据 Fa 自动计算，手动设置无效

参数

[in] state_jac 状态方程雅可比矩阵

◆ setJh()

template<typename FloatTp, int StateDim, int MeasureDim>

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

inline

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

弃用: 雅可比矩阵会在 correct() 中根据 Fh 自动计算，手动设置无效

参数

[in] observe_jac 观测方程雅可比矩阵

◆ setV()

template<typename FloatTp, int StateDim, int MeasureDim>

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

inline

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

参见: 1.2 观测方程线性化

参数

[in] measure_jac 测量噪声协方差雅可比矩阵

◆ setW()

template<typename FloatTp, int StateDim, int MeasureDim>

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

inline

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

参见: 1.1 状态方程线性化

参数

[in] process_jac 过程噪声协方差雅可比矩阵

类成员变量说明

◆ Fa

template<typename FloatTp, int StateDim, int MeasureDim>

std::function<cv::Matx<FloatTp, StateDim, 1>(const cv::Matx<FloatTp, StateDim, 1> &)> rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::Fa

protected

非线性的离散状态方程

◆ Fh

template<typename FloatTp, int StateDim, int MeasureDim>

std::function<cv::Matx<FloatTp, MeasureDim, 1>(const cv::Matx<FloatTp, StateDim, 1> &)> rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::Fh

protected

非线性的离散观测方程

◆ Ja

template<typename FloatTp, int StateDim, int MeasureDim>

cv::Matx<FloatTp, StateDim, StateDim> rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::Ja

protected

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

◆ Jat

template<typename FloatTp, int StateDim, int MeasureDim>

cv::Matx<FloatTp, StateDim, StateDim> rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::Jat

protected

状态方程雅可比矩阵的转置 \(J_A^T\)

◆ Jh

template<typename FloatTp, int StateDim, int MeasureDim>

cv::Matx<FloatTp, MeasureDim, StateDim> rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::Jh

protected

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

◆ Jht

template<typename FloatTp, int StateDim, int MeasureDim>

cv::Matx<FloatTp, StateDim, MeasureDim> rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::Jht

protected

观测方程雅可比矩阵的转置 \(J_H^T\)

◆ V

template<typename FloatTp, int StateDim, int MeasureDim>

cv::Matx<FloatTp, MeasureDim, MeasureDim> rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::V

protected

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

◆ Vt

template<typename FloatTp, int StateDim, int MeasureDim>

cv::Matx<FloatTp, MeasureDim, MeasureDim> rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::Vt

protected

测量噪声协方差雅可比矩阵的转置 \(V^T\)

◆ W

template<typename FloatTp, int StateDim, int MeasureDim>

cv::Matx<FloatTp, StateDim, StateDim> rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::W

protected

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

◆ Wt

template<typename FloatTp, int StateDim, int MeasureDim>

cv::Matx<FloatTp, StateDim, StateDim> rm::ExtendedKalmanFilter< FloatTp, StateDim, MeasureDim >::Wt

protected

过程噪声协方差雅可比矩阵的转置 \(W^T\)

Public 成员函数

Protected 类型

静态 Protected 成员函数

Protected 属性

详细描述

成员类型定义说明

◆ StateFunction

构造及析构函数说明

◆ ExtendedKalmanFilter()

成员函数说明

◆ calcJ()

◆ correct()

◆ predict()

◆ setFa()

◆ setFh()

◆ setJa()

◆ setJh()

◆ setV()

◆ setW()

类成员变量说明

◆ Fa

◆ Fh

◆ Ja

◆ Jat

◆ Jh

◆ Jht

◆ V

◆ Vt

◆ W

◆ Wt