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

卡尔曼滤波器 更多...

#include <rmvl/algorithm/kalman.hpp>

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

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

Public 成员函数
	KalmanFilter ()
	构造新的 KalmanFilter 对象
void	setA (const cv::Matx< Tp, StateDim, StateDim > &state_tf)
	设置状态转移矩阵 \(A\)
void	setH (const cv::Matx< Tp, MeasureDim, StateDim > &observe_tf)
	设置观测矩阵 \(H\)
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::KalmanFilter< Tp, StateDim, MeasureDim >

卡尔曼滤波器

模板参数

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

构造及析构函数说明

KalmanFilter()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

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

inline

构造新的 KalmanFilter 对象

成员函数说明

correct()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

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

inline

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

公式如下

\[\begin{align}K_k&=P_k^-H^T\left(HP_k^-H^T+R\right)^{-1}\\\hat{\pmb x}_k&=\hat{\pmb x}_k^-+K\left(\pmb z_k-H\hat{\pmb x}_k^-\right)\\P_k&=\left(I-K_kH\right)P_k^-\end{align}\]

参数

[in]

zk

观测量

返回

后验状态估计

predict()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

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

inline

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

公式如下

\[\begin{align}\hat{\pmb x}_k^-&=A\hat{\pmb x}_{k-1}\\P_k^-&=AP_{k-1}A^T+Q\end{align}\]

返回

先验状态估计

setA()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

void rm::KalmanFilter< Tp, StateDim, MeasureDim >::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

状态转移矩阵

setH()

template<typename Tp , unsigned StateDim, unsigned MeasureDim>

void rm::KalmanFilter< Tp, StateDim, MeasureDim >::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}\]

参数

[in]

observe_tf

观测矩阵

---

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

• rmvl/algorithm/kalman.hpp