| RMVL 2.4.0 Robotic Manipulation and Vision Library

载入中...
搜索中...
未找到
扩展卡尔曼滤波

扩展卡尔曼滤波

作者
  赵曦

日期
  2024/04/19

版本
  1.0

上一篇教程：卡尔曼滤波
下一篇教程：离散傅里叶变换

相关类 rm::ExtendedKalmanFilter

\[\def\red#1{\color{red}{#1}} \def\teal#1{\color{teal}{#1}} \def\green#1{\color{green}{#1}} \def\transparent#1{\color{transparent}{#1}} \def\orange#1{\color{orange}{#1}} \def\Var{\mathrm{Var}} \def\Cov{\mathrm{Cov}} \def\tr{\mathrm{tr}} \def\fml#1{\text{(#1)}} \def\ptl#1#2{\frac{\partial#1}{\partial#2}} \]

在阅读本教程前，请确保已经熟悉标准的 卡尔曼滤波 ，因为核心公式不变，只是在原来的基础上增加了非线性函数线性化的部分。

## 1 非线性函数的线性化

对于一个线性系统，可以用状态空间方程描述其运动过程

\[\begin{align}\dot{\boldsymbol x}&=A\boldsymbol x+B\boldsymbol u\\\boldsymbol y&=C\boldsymbol x\end{align}\tag{1-1}\]

离散化，并考虑噪声后可以写为

\[\begin{align}\dot{\boldsymbol x}_k&=A\boldsymbol x_{k-1}+B\boldsymbol u_{k-1}+\boldsymbol w_{k-1}&&\boldsymbol w_{k-1}\sim N(0,Q)\tag{1-2a}\\ \boldsymbol z_k&=H\boldsymbol x_{k-1}+\boldsymbol v_k&&\boldsymbol v_k\sim N(0,R)\tag{1-2b}\end{align}\]

但对于一个非线性系统，我们就无法使用矩阵来表示了，我们需要写为

\[\left\{\begin{align}\dot{\boldsymbol x}_k&=\boldsymbol f_A(\boldsymbol x_{k-1},\boldsymbol u_{k-1},\boldsymbol w_{k-1})\\ \boldsymbol z_k&=\boldsymbol f_H(\boldsymbol x_{k-1},\boldsymbol v_{k-1})\end{align}\right.\tag{1-3}\]

其中， \(\boldsymbol f_A\) 和 \(\boldsymbol f_H\) 都为非线性函数。我们在非线性函数中同样考虑了噪声，但是对于状态量以及观测量本身的噪声而言，正态分布的随机变量通过非线性系统后就不再服从正态分布了。因此我们可以利用 泰勒展开 ，将非线性系统线性化，即

\[f(x)\approx f(x_0)+\frac{\mathrm df}{\mathrm dx}(x-x_0)\tag{1-4}\]

对于多元函数而言，泰勒展开可以写为

\[f(x,y,z)\approx f(x_0,y_0,z_0)+\begin{bmatrix}f'_x(x_0,y_0,z_0)&f'_y(x_0,y_0,z_0)&f'_z(x_0,y_0,z_0)\end{bmatrix}\begin{bmatrix}x-x_0\\y-y_0\\z-z_0\end{bmatrix}\tag{1-5a}\]

即

\[f(\boldsymbol x)\approx f(\boldsymbol x_0)+\ptl fx(\boldsymbol x-\boldsymbol x_0)=f(\boldsymbol x_0)+\nabla f(\boldsymbol x_0)(\boldsymbol x-\boldsymbol x_0)\tag{1-5b}\]

### 1.1 状态方程线性化

对公式 \(\fml{1-2a}\) 在 \(\hat{\boldsymbol x}_{k-1}\) 处进行线性化，即选取 \(\text{k-1}\) 时刻的后验状态估计作为展开点，有

\[\boldsymbol x_k=\boldsymbol f_A(\hat{\boldsymbol x}_{k-1},\boldsymbol u_{k-1},\boldsymbol w_{k-1})+J_A(\boldsymbol x_{k-1}-\hat x_{k-1})+W\boldsymbol w_{k-1}\tag{1-6}\]

令 \(\boldsymbol w_{k-1}=\boldsymbol 0\)，则 \(f_A(\hat{\boldsymbol x}_{k-1},\boldsymbol u_{k-1},\boldsymbol w_{k-1})=f_A(\hat{\boldsymbol x}_{k-1},\boldsymbol u_{k-1},\boldsymbol 0)\stackrel{\triangle}=\tilde{\boldsymbol x}_{k-1}\)，有

\[\red{\boldsymbol x_k=\tilde{\boldsymbol x}_{k-1}+J_A(\boldsymbol x_{k-1}-\hat x_{k-1})+W\boldsymbol w_{k-1}\qquad W\boldsymbol w_{k-1}\sim N(0,WQW^T)\tag{1-7}}\]

其中

\[\begin{align}J_A&=\left.\ptl{f_A}{\boldsymbol x}\right|_{(\hat{\boldsymbol x}_{k-1},\boldsymbol u_{k-1})}=\begin{bmatrix}\ptl{{f_A}_1}{x_1}&\ptl{{f_A}_1}{x_2}&\cdots&\ptl{{f_A}_1}{x_n}\\\ptl{{f_A}_2}{x_1}&\ptl{{f_A}_2}{x_2}&\cdots&\ptl{{f_A}_2}{x_n}\\\vdots&\vdots&\ddots&\vdots\\\ptl{{f_A}_n}{x_1}&\ptl{{f_A}_n}{x_2}&\cdots&\ptl{{f_A}_n}{x_n}\end{bmatrix}\\ W&=\left.\ptl{f_A}{\boldsymbol w}\right|_{(\hat{\boldsymbol w}_{k-1},\boldsymbol u_{k-1})}\end{align}\]

### 1.2 观测方程线性化

对公式 \(\fml{1-2b}\) 在 \(\hat{\boldsymbol x}_k\) 处进行线性化，有

\[\boldsymbol z_k=\boldsymbol f_H(\tilde{\boldsymbol x}_k,\boldsymbol v_k)+J_H(\boldsymbol x_k-\tilde x_k)+V\boldsymbol v_k\tag{1-8}\]

令 \(\boldsymbol v_k=\boldsymbol 0\)，则 \(f_H(\tilde{\boldsymbol x}_k,\boldsymbol v_k)=f_H(\tilde{\boldsymbol x}_k,\boldsymbol 0)\stackrel{\triangle}=\tilde{\boldsymbol z}_k\)，有

\[\red{\boldsymbol z_k=\tilde{\boldsymbol z}_k+J_H(\boldsymbol x_k-\tilde x_k)+V\boldsymbol v_k\qquad V\boldsymbol v_k\sim N(0,VRV^T)\tag{1-9}}\]

其中

\[J_H=\left.\ptl{f_H}{\boldsymbol x}\right|_{\tilde{\boldsymbol x}_k},\qquad V=\left.\ptl{f_H}{\boldsymbol v}\right|_{\tilde{\boldsymbol x}_k}\]

## 2 扩展卡尔曼滤波

### 2.1 公式汇总

根据卡尔曼滤波的 1.7 汇总 可以相应的改写非线性系统下的卡尔曼滤波公式，从而得到如下的扩展卡尔曼滤波公式。

① 预测

1. 先验状态估计

   \[\hat{\boldsymbol x}_k^-=\boldsymbol f_A(\boldsymbol x_{k-1},\boldsymbol u_{k-1},\boldsymbol 0)\]

2. 计算先验误差协方差

   \[P_k^-=J_AP_{k-1}J_A^T+WQW^T\]

② 校正（更新）

1. 计算卡尔曼增益

   \[K_k=P_k^-J_H^T\left(J_HP_k^-J_H^T+VRV^T\right)^{-1}\]

2. 后验状态估计

   \[\hat{\boldsymbol x}_k=\hat{\boldsymbol x}_k^-+K_k\left[\boldsymbol z_k-\boldsymbol f_H(\hat{\boldsymbol x}_k^-,\boldsymbol 0)\right]\]

3. 更新后验误差协方差

   \[P_k=(I-K_kJ_H)P_k^-\]

### 2.2 EKF 模块的使用

下面拿扩展卡尔曼模块单元测试的内容举例子

#include <cstdio>
#include <random>
#include <rmvl/algorithm/kalman.hpp>
int main() {
// 状态量 x = [ cx, cy, θ, ω, r ]ᵀ
//
// 状态方程
// ┌ cx ┌ 1 0 0 0 0 ┐
// │ cy │ 0 1 0 0 0 │
// F = │ θ + ωT Ja = │ 0 0 1 T 0 │ = A
// │ ω │ 0 0 0 1 0 │
// └ r └ 0 0 0 0 1 ┘
//
// 观测量 z = [ px, py, θ ]ᵀ
//
// 观测方程
// ┌ cx + rcosθ ┌ 1 0 -rsinθ 0 cosθ ┐
// H = │ cy + rsinθ Jh = │ 0 1 rcosθ 0 sinθ │
// └ θ └ 0 0 1 0 0 ┘
// 正态分布噪声
std::default_random_engine ng;
std::normal_distribution<double> err{0, 1};
// 创建扩展卡尔曼滤波
rm::EKF53d ekf;
ekf.init({0, 0, 0, 0, 150}, 1e5);
ekf.setQ(1e-1 * cv::Matx<double, 5, 5>::eye());
ekf.setR(cv::Matx33d::diag({1e-3, 1e-3, 1e-3}));
double t{0.01};
// 设置状态方程（这里的例子是线性的，但一般都是非线性的）
ekf.setFa([=](const cv::Matx<double, 5, 1> &x) -> cv::Matx<double, 5, 1> {
return {x(0),
x(1),
x(2) + x(3) * t,
x(3),
x(4)};
});
// 设置观测方程
ekf.setFh([=](const cv::Matx<double, 5, 1> &x) -> cv::Matx<double, 3, 1> {
return {x(0) + x(4) * std::cos(x(2)),
x(1) + x(4) * std::sin(x(2)),
x(2)};
});
while (true) {
// 预测部分，设置状态方程 Jacobi 矩阵，获取先验状态估计
ekf.setJa({1, 0, 0, 0, 0,
0, 1, 0, 0, 0,
0, 0, 1, t, 0,
0, 0, 0, 1, 0,
0, 0, 0, 0, 1});
auto x_ = ekf.predict();
// 更新部分，设置观测方程 Jacobi 矩阵，获取后验状态估计
ekf.setJh({1, 0, -x_(4) * std::sin(x_(2)), 0, std::cos(x_(2)),
0, 1, x_(4) * std::cos(x_(2)), 0, std::sin(x_(2)),
0, 0, 1, 0, 0});
// 以 20 为半径，0.02/T 为角速度的圆周运动（图像上是顺时针），并人为加上观测噪声
auto x = ekf.correct({500 + 200 * std::cos(0.02 * i) + err(ng),
500 + 200 * std::sin(0.02 * i) + err(ng),
0.02 * i + 0.01 * err(ng)});
printf("x = [%.3f, %.3f, %.3f, %.3f, %.3f]\n", x(0), x(1), x(2), x(3), x(4));
}
}
rm::ExtendedKalmanFilter::setJh
void setJh(const cv::Matx< Tp, MeasureDim, StateDim > &observe_jac)
设置观测方程雅可比矩阵
定义 kalman.hpp:249
rm::ExtendedKalmanFilter::setFa
void setFa(const std::function< cv::Matx< Tp, StateDim, 1 >(const cv::Matx< Tp, StateDim, 1 > &)> &state_func)
设置非线性的离散状态方程
定义 kalman.hpp:256
rm::ExtendedKalmanFilter::predict
auto predict()
扩展卡尔曼滤波的预测部分，包括状态量的先验估计和误差协方差的先验估计
定义 kalman.hpp:288
rm::ExtendedKalmanFilter::correct
auto correct(const cv::Matx< Tp, MeasureDim, 1 > &zk)
扩展卡尔曼滤波的校正部分，包含卡尔曼增益的计算、状态量的后验估计和误差协方差的后验估计
定义 kalman.hpp:306
rm::ExtendedKalmanFilter::setFh
void setFh(const std::function< cv::Matx< Tp, MeasureDim, 1 >(const cv::Matx< Tp, StateDim, 1 > &)> &observe_func)
设置非线性的离散观测方程
定义 kalman.hpp:263
rm::ExtendedKalmanFilter::setJa
void setJa(const cv::Matx< Tp, StateDim, StateDim > &state_jac)
设置状态方程雅可比矩阵
定义 kalman.hpp:242
rm::KalmanFilterStaticDatas::setQ
void setQ(const cv::Matx< Tp, StateDim, StateDim > &process_err)
设置过程噪声协方差矩阵
定义 kalman.hpp:90
rm::KalmanFilterStaticDatas::setR
void setR(const cv::Matx< Tp, MeasureDim, MeasureDim > &measure_err)
设置测量噪声协方差矩阵
定义 kalman.hpp:83
rm::KalmanFilterStaticDatas::init
void init(const cv::Matx< Tp, StateDim, 1 > &x0, Tp error)
初始化状态以及对应的误差协方差矩阵（常数对角矩阵）
定义 kalman.hpp:60
rm::EKF53d
ExtendedKalmanFilter< double, 5U, 3U > EKF53d
5 × 3 扩展卡尔曼滤波器
定义 kalman.hpp:343