轻量级 cv::Matx 的卡尔曼滤波模块更多...

#include <rmvl/core/kalman.hpp>

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

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

Public 成员函数
	KalmanFilter ()
	构造新的 KalmanFilter 对象更多...

void	init (const cv::Matx< Tp, StateDim, 1 > &state, Tp error)
	初始化状态以及对应的误差协方差矩阵（常数对角矩阵）更多...

void	init (const cv::Matx< Tp, StateDim, 1 > &state, const cv::Matx< Tp, StateDim, 1 > &error)
	初始化状态以及对应的误差协方差矩阵（对角矩阵）更多...

void	setA (const cv::Matx< Tp, StateDim, StateDim > &state_tf)
	设置状态转移矩阵 `A` 更多...

void	setB (const cv::Matx< Tp, StateDim, ControlDim > &control_matrix)
	设置控制矩阵 `B` 更多...

void	setH (const cv::Matx< Tp, MeasureDim, StateDim > &observe_tf)
	设置观测转换矩阵 `H` 更多...

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` 更多...

auto	predict (const cv::Matx< Tp, ControlDim, 1 > &control_vec)
	含控制量的卡尔曼滤波的预测部分，包括状态向量的先验估计和误差协方差的先验估计更多...

auto	predict ()
	不含控制量的卡尔曼滤波的预测部分，包括状态向量的先验估计和误差协方差的先验估计更多...

auto	correct (const cv::Matx< Tp, MeasureDim, 1 > &measurement)
	卡尔曼滤波器的校正部分，包括卡尔曼增益 `K` 的计算、状态向量的后验估计和误差协方差的校正更多...

详细描述

template<typename Tp, uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>
class rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >

轻量级 cv::Matx 的卡尔曼滤波模块

模板参数

Tp	数据类型
StateDim	状态向量的维度，类型是 `uint16_t`
MeasureDim	观测向量的维度，类型是 `uint16_t`
ControlDim	控制向量的维度，类型是 `uint16_t`，默认为 `1`

构造及析构函数说明

◆ KalmanFilter()

template<typename Tp , uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>

rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >::KalmanFilter ( )

inline

构造新的 KalmanFilter 对象

成员函数说明

◆ correct()

template<typename Tp , uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>

auto rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >::correct ( const cv::Matx< Tp, MeasureDim, 1 > & measurement )

inline

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

更新部分公式如下

\[\begin{align}K&=P^-H^T\left(HP^-H^T+R\right)^{-1}\\\hat{\pmb x}&= \hat{\pmb x}^-+K\left(\pmb z-H\hat{\pmb x}^-\right)\\P&=\left(I-KH\right)P^-\end{align}\]

参数

[in] measurement 观测向量

返回: 后验状态估计

◆ init() [1/2]

template<typename Tp , uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>

void rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >::init	(	const cv::Matx< Tp, StateDim, 1 > &	state,
		const cv::Matx< Tp, StateDim, 1 > &	error
	)

inline

初始化状态以及对应的误差协方差矩阵（对角矩阵）

参数

[in]	state	初始化的状态向量
[in]	error	状态误差矩阵的对角线元素

函数调用图:

◆ init() [2/2]

template<typename Tp , uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>

void rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >::init	(	const cv::Matx< Tp, StateDim, 1 > &	state,
		Tp	error
	)

inline

初始化状态以及对应的误差协方差矩阵（常数对角矩阵）

参数

[in]	state	初始化的状态向量
[in]	error	状态误差系数

函数调用图:

◆ predict() [1/2]

template<typename Tp , uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>

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

inline

不含控制量的卡尔曼滤波的预测部分，包括状态向量的先验估计和误差协方差的先验估计

参见: predict(const cv::Matx<Tp, ControlDim, 1> &control)

注解: 公式中 \(B\pmb u = \pmb 0\)

返回: 先验状态估计

◆ predict() [2/2]

template<typename Tp , uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>

auto rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >::predict ( const cv::Matx< Tp, ControlDim, 1 > & control_vec )

inline

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

预测部分公式如下

\[\begin{align}\hat{\pmb x}^-&=A\hat{\pmb x}+B\pmb u\\P^-&=APA^T+Q\end{align}\]

参数

[in] control_vec 控制向量，即公式中的 \(\pmb u\)

返回: 先验状态估计

◆ setA()

template<typename Tp , uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>

void rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >::setA ( const cv::Matx< Tp, StateDim, StateDim > & state_tf )

inline

设置状态转移矩阵 A

包含 x 方向位置、y 方向位置、x 方向速度和 y 方向速度的运动过程一般可以描述为

\[\left\{\begin{align}x_{n+1}&=x_n+{v_x}_nt+\frac12{a_x}_nt^2\\y_{n+1}&=y_n+{v_y}_nt +\frac12{a_y}_nt^2\\{v_x}_{n+1}&={v_x}_n+{a_x}_nt\\{v_y}_{n+1}&={v_y}_n+{a_y}_nt \end{align}\right.\tag1\]

使用矩阵表示为

\[\begin{bmatrix}x_{n+1}\\y_{n+1}\\{v_x}_{n+1}\\ {v_y}_{n+1}\end{bmatrix}=\begin{bmatrix}1&0&t&0\\0&1&0&t\\0&0&1&0\\0&0&0&1\end{bmatrix} \begin{bmatrix}x_n\\y_n\\{v_x}_n\\{v_y}_n\end{bmatrix}+\begin{bmatrix}\frac12t^2&0\\ 0&\frac12t^2\\t&0\\0&t\end{bmatrix}\begin{bmatrix}{a_x}_n\\{a_y}_n\end{bmatrix}\tag2\]

即可以写成

\[\pmb{x}_{n+1}=A\pmb{x}_n+B\pmb{u}_n\tag3\]

在这条公式中

\(A\) 是状态转移矩阵
\(\pmb x\) 是状态向量
\(n\) 和 \(n+1\) 表示当前帧和上一帧的数据
\(B\) 是控制矩阵
\(\pmb u\) 是控制向量

参数

[in] state_tf 状态转移矩阵

◆ setB()

template<typename Tp , uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>

void rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >::setB ( const cv::Matx< Tp, StateDim, ControlDim > & control_matrix )

inline

设置控制矩阵 B

输入直接对状态向量的作用，由控制矩阵 B 定义

参见: setA

参数

[in] control_matrix 控制矩阵

◆ setH()

template<typename Tp , uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>

void rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >::setH ( const cv::Matx< Tp, MeasureDim, StateDim > & observe_tf )

inline

设置观测转换矩阵 H

若状态向量包含以下内容： \([p, v, a]^T\) ，然而观测向量仅包含 \([p, v]^T\)，在这种情况下，需要使用一个观测转换矩阵 \(H_{2\times3}\)。在上述例子中克表示为

\[\begin{bmatrix}p\\v\end{bmatrix}=\begin{bmatrix}1&0&0\\0&1&0\end{bmatrix} \begin{bmatrix}p\\v\\a\end{bmatrix}\]

注解: H 若不加以设置，则默认是单位矩阵 \(I\)

参数

[in] observe_tf 观测转换矩阵

◆ setP()

template<typename Tp , uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>

void rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >::setP ( const cv::Matx< Tp, StateDim, StateDim > & state_err )

inline

设置状态误差协方差矩阵 P

参数

[in] state_err 状态误差协方差矩阵 P

◆ setQ()

template<typename Tp , uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>

void rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >::setQ ( const cv::Matx< Tp, StateDim, StateDim > & process_err )

inline

设置过程噪声协方差矩阵 Q

参数

[in] process_err 过程噪声协方差矩阵 Q

◆ setR()

template<typename Tp , uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>

void rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >::setR ( const cv::Matx< Tp, MeasureDim, MeasureDim > & measure_err )

inline

设置测量噪声协方差矩阵 R

参数

[in] measure_err 测量噪声协方差矩阵 R

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

rmvl/core/kalman.hpp

Public 成员函数

详细描述

template<typename Tp, uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1> class rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >

构造及析构函数说明

◆ KalmanFilter()

成员函数说明

◆ correct()

◆ init() [1/2]

◆ init() [2/2]

◆ predict() [1/2]

◆ predict() [2/2]

◆ setA()

◆ setB()

◆ setH()

◆ setP()

◆ setQ()

◆ setR()

template<typename Tp, uint16_t StateDim, uint16_t MeasureDim, uint16_t ControlDim = 1>
class rm::KalmanFilter< Tp, StateDim, MeasureDim, ControlDim >