扩展卡尔曼滤波器 更多...
#include <rmvl/core/kalman.hpp>
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) |
设置非线性的离散状态方程 \(\pmb f_A(\pmb x)\) | |
void | setFh (const std::function< cv::Matx< Tp, MeasureDim, 1 >(const cv::Matx< Tp, StateDim, 1 > &)> &observe_func) |
设置非线性的离散观测方程 \(\pmb f_H(\pmb 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) |
扩展卡尔曼滤波的校正部分,包含卡尔曼增益的计算、状态量的后验估计和误差协方差的后验估计 | |
![]() | |
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\) | |
额外继承的成员函数 | |
![]() | |
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\) | |
扩展卡尔曼滤波器
Tp | 数据类型 |
StateDim | 状态量个数 |
MeasureDim | 观测量个数 |
|
inline |
构造新的 ExtendedKalmanFilter 对象
|
inline |
扩展卡尔曼滤波的校正部分,包含卡尔曼增益的计算、状态量的后验估计和误差协方差的后验估计
公式如下
\[\begin{align}K_k&=P_k^-J_H^T\left(J_HP_k^-J_H^T+VRV^T\right)^{-1}\\\hat{\pmb x} &=\hat{\pmb x}_k^-+K_k\left[\pmb z_k-\pmb f_H(\hat{\pmb x}_k^-)\right]\\P_k&=\left(I-K_kJ_H \right)P_k^-\end{align}\]
[in] | zk | 观测量 |
|
inline |
扩展卡尔曼滤波的预测部分,包括状态量的先验估计和误差协方差的先验估计
公式如下
\[\begin{align}\hat{\pmb x_k}^-&=\pmb f(\hat{\pmb x}_{k-1})\\ P_k^-&=J_AP_{k-1}J_A^T+WQW^T\end{align}\]
|
inline |
设置非线性的离散状态方程 \(\pmb f_A(\pmb x)\)
[in] | state_func | 状态方程 |
|
inline |
设置非线性的离散观测方程 \(\pmb f_H(\pmb x)\)
[in] | observe_func | 观测方程 |
|
inline |
设置状态方程雅可比矩阵 \(J_A\)
[in] | state_jac | 状态方程雅可比矩阵 |
|
inline |
设置观测方程雅可比矩阵 \(J_H\)
[in] | observe_jac | 观测方程雅可比矩阵 |
|
inline |
|
inline |