\[\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 汇总可以相应的改写非线性系统下的卡尔曼滤波公式，从而得到如下的扩展卡尔曼滤波公式。

① 预测

先验状态估计
\[\hat{\boldsymbol x}_k^-=\boldsymbol f_A(\boldsymbol x_{k-1},\boldsymbol u_{k-1},\boldsymbol 0)\]
计算先验误差协方差
\[P_k^-=J_AP_{k-1}J_A^T+WQW^T\]

② 校正（更新）

计算卡尔曼增益
\[K_k=P_k^-J_H^T\left(J_HP_k^-J_H^T+VRV^T\right)^{-1}\]
后验状态估计
\[\hat{\boldsymbol x}_k=\hat{\boldsymbol x}_k^-+K_k\left[\boldsymbol z_k-\boldsymbol f_H(\hat{\boldsymbol x}_k^-,\boldsymbol 0)\right]\]
更新后验误差协方差
\[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));
    }
}