|
| | 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 |
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}\]
- 参数
-
- 返回
- 后验状态估计
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\) |
- 返回
- 先验状态估计
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\) 是控制向量
- 参数
-
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\)
- 参数
-
1.11.0