| RMVL 2.4.0 Robotic Manipulation and Vision Library

载入中...
搜索中...
未找到
Public 成员函数 | 所有成员列表
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{\boldsymbol x}\)
cv::Matx< Tp, StateDim, 1 > | x_
| 状态的先验估计 \(\hat{\boldsymbol x}^-\)
cv::Matx< Tp, MeasureDim, 1 > | z
| 观测向量 \(\boldsymbol 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{\boldsymbol x}_k&=\hat{\boldsymbol x}_k^-+K\left(\boldsymbol z_k-H\hat{\boldsymbol 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{\boldsymbol x}_k^-&=A\hat{\boldsymbol 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\]

即可以写成

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

在这条公式中

- \(A\) 是状态转移矩阵
- \(\boldsymbol x\) 是状态向量
- \(n\) 和 \(n+1\) 表示当前帧和上一帧的数据
- \(B\) 是控制矩阵
- \(\boldsymbol 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 | 观测矩阵