| RMVL 2.4.0 Robotic Manipulation and Vision Library

载入中...
搜索中...
未找到
Public 成员函数 | 所有成员列表
rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim > 模板类 参考
数学、算法模块 » 数据与信号处理 » 卡尔曼滤波模块

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

`#include <rmvl/algorithm/kalman.hpp>`

类 rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim > 继承关系图:
rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim > 的协作图:

## Public 成员函数
| ExtendedKalmanFilter ()
| 构造新的 ExtendedKalmanFilter 对象
void | setJa (const cv::Matx< Tp, StateDim, StateDim > &state_jac)
| 设置状态方程雅可比矩阵 \(J_A\)
void | setJh (const cv::Matx< Tp, MeasureDim, StateDim > &observe_jac)
| 设置观测方程雅可比矩阵 \(J_H\)
void | setFa (const std::function< cv::Matx< Tp, StateDim, 1 >(const cv::Matx< Tp, StateDim, 1 > &)> &state_func)
| 设置非线性的离散状态方程 \(\boldsymbol f_A(\boldsymbol x)\)
void | setFh (const std::function< cv::Matx< Tp, MeasureDim, 1 >(const cv::Matx< Tp, StateDim, 1 > &)> &observe_func)
| 设置非线性的离散观测方程 \(\boldsymbol f_H(\boldsymbol x)\)
void | setW (const cv::Matx< Tp, StateDim, StateDim > &process_jac)
| 设置过程噪声协方差雅可比矩阵 \(W\)
void | setV (const cv::Matx< Tp, MeasureDim, MeasureDim > &measure_jac)
| 设置测量噪声协方差雅可比矩阵 \(V\)
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::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >

扩展卡尔曼滤波器

模板参数

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

## 构造及析构函数说明

## ◆ ExtendedKalmanFilter()

template<typename Tp, unsigned StateDim, unsigned MeasureDim>

rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >::ExtendedKalmanFilter | ( |  | ) |
| inline

构造新的 ExtendedKalmanFilter 对象

## 成员函数说明

## ◆ correct()

template<typename Tp, unsigned StateDim, unsigned MeasureDim>

auto rm::ExtendedKalmanFilter< Tp, StateDim, MeasureDim >::correct | ( | const cv::Matx< Tp, 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 Tp, unsigned StateDim, unsigned MeasureDim>

auto rm::ExtendedKalmanFilter< Tp, 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 Tp, unsigned StateDim, unsigned MeasureDim>

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

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

参数

  [in] | state_func | 状态方程

## ◆ setFh()

template<typename Tp, unsigned StateDim, unsigned MeasureDim>

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

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

参数

  [in] | observe_func | 观测方程

## ◆ setJa()

template<typename Tp, unsigned StateDim, unsigned MeasureDim>

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

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

参数

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

## ◆ setJh()

template<typename Tp, unsigned StateDim, unsigned MeasureDim>

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

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

参数

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

## ◆ setV()

template<typename Tp, unsigned StateDim, unsigned MeasureDim>

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

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

参见
  1.2 观测方程线性化

参数

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

## ◆ setW()

template<typename Tp, unsigned StateDim, unsigned MeasureDim>

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

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

参见
  1.1 状态方程线性化

参数

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