4 概率机器人 Probabilistic Robotics 扩展卡尔曼滤波算法

1 前提介绍

• 扩展卡尔曼滤波顾名思义是卡尔曼滤波的扩展形式（卡尔曼滤波教程），但是在哪些方面进行扩展呢？这就要从卡尔曼滤波的三个前提条件入手：
  1. 状态转移函数必须是线性函数:$x_t=A_t{x}_{t-1}+B_t{u}_t+{ϵ}_t$
  2. 测量函数必须是线性函数:$z_t=C_t{x}_t+{\delta }_t$
  3. 初始的置信度函数服从正态分布:$b(x_0)\sim N({\mu }_0,{\Sigma }_0)$
• 扩展卡尔曼滤波对第$1,2$个前提条件进行扩展：
  1. 状态转移函数不必是线性函数$x_t=g(u_t,x_{t-1})+{ϵ}_t$
  2. 测量函数必须不必是线性函数:$z_t=h(x_t)+{\delta }_t$
• 若$X$服从正态分布$X\sim N(x;\mu ,{\sigma }^2)$,那么变换后的变量$Y$服从怎样的分布？
• • 线性变换$Y=aX+b$，那么变量$Y$也服从正态分布$Y\sim N(y;a\mu +b,a^2{\sigma }^2)$

• • 非线性变换$Y=g(X)$,那么变量$Y$将不再服从正态分布，且解析形式很难求得

• 扩展卡尔曼滤波与卡尔曼滤波状态转移函数和测量函数对比：
  • 用函数$g$代替矩阵$A_t,B_t$
  • 用函数$h$代替矩阵$C_t$
• 对于任意函数$g,h$将会导致置信度$bel(x)$不再服从正态分布，那么置信度更新将不再存在封闭解，换句话说贝叶斯滤波将不能进行
• 那么为了克服上述问题，只能退而求其次，采用将上述两个非线性函数$g,h$进行线性化

2 通过泰勒展式进行线性化

• 泰勒展开式与线性化：
  $$\begin{gathered} \text{在}x=a\text{出泰勒展开形式：} \\ f(x)=f(a)+\frac{f^{\prime }(a)}{1!}(x-a)+\frac{f^{(2)}(a)}{2!}(x-a{)}^2+\cdots +\frac{f^{(n)}(a)}{n!}(x-a{)}^n+\cdots \\ \downarrow \text{仅保留线性部分} \\ f(x)\approx f(a)+\frac{f^{\prime }(a)}{1!}(x-a) \end{gathered}$$
• 如果$X$服从正态分布$X\sim N(x;\mu ,{\sigma }^2)$，$g(x)$是非线性函数，那么为了线性化$g(x)$，应该在什么位置对$g(x)$进行一阶泰勒展开呢？
  • 答案是应该在$X$的均值出进行展开$(x=\mu )$
  • 这是因为$x$等于均值$\mu$的概率最大
  • 变换如下图：
    1. 右下角为$X$服从正态分布$X\sim N(x;\mu ,{\sigma }^2)$，$x$等于均值$\mu$的概率最大(峰值)
    2. 右上角为对$x$进行非线性变换$g(x)$,虚线是在$x=\mu$处的一阶泰勒展开式，用来近似实线部分
    3. 左上角虚线为通过线性化$g(x)$，求得的$g(x)$的概率分布；实线为实际的概率分布，可见误差在允许的范围内

• 线性化状态转移函数$x_t=g(u_t,x_{t-1})+{ϵ}_t$
  • 对于高斯分布，后验概率${\mu }_{t-1}$是状态变量$x_{t-1}$最可能的状态
  • 所以对$x_t=g(u_t,x_{t-1})+{ϵ}_t$在${\mu }_{t-1}$处展开(即$a={\mu }_{t-1}$)
    $$\begin{gathered} g(u_t,x_{t-1})\approx g(u_t,{\mu }_{t-1})+g^{\prime }(u_t,{\mu }_{t-1})(x_{t-1}-{\mu }_{t-1}) \\ \downarrow \text{定义：}{G}_t:=g^{\prime }(u_t,{\mu }_{t-1})\text{雅克比矩阵} \\ g(u_t,x_{t-1})\approx g(u_t,{\mu }_{t-1})+G_t(x_{t-1}-{\mu }_{t-1}) \\ \downarrow \\ {x}_t\approx g(u_t,{\mu }_{t-1})+G_t(x_{t-1}-{\mu }_{t-1})+{ϵ}_t \\ \downarrow \\ {x}_t\sim N(x_t;g(u_t,{\mu }_{t-1})+G_t(x_{t-1}-{\mu }_{t-1}),R_t) \\ \downarrow \\ \begin{array}{rl}& p(x_t|u_t,x_{t-1})\\ & =det(2\pi R_t{)}^{-\frac{1}{2}}\exp \left\{-\frac{1}{2}(x_t-g(u_t,{\mu }_{t-1})-G_t(x_{t-1}-{\mu }_{t-1}){)}^T{R}_t^{-1}(x_t-g(u_t,{\mu }_{t-1})-G_t(x_{t-1}-{\mu }_{t-1}))\right\}\end{array} \\ \text{雅克比矩阵：}{G}_t=g^{\prime }(u_t,{\mu }_{t-1})={\left[\begin{array}{cccc}\frac{\partial g_1}{\partial x_1}& \frac{\partial g_1}{\partial x_2}& \cdots & \frac{\partial g_1}{\partial x_n}\\ \frac{\partial g_2}{\partial x_1}& \frac{\partial g_2}{\partial x_2}& \cdots & \frac{\partial g_2}{\partial x_n}\\ ⋮& ⋮& ⋮& ⋮\\ \frac{\partial g_n}{\partial x_1}& \frac{\partial g_n}{\partial x_2}& \cdots & \frac{\partial g_n}{\partial x_n}\end{array}\right]}_{x_{t-1}={\mu }_{t-1}} \end{gathered}$$
• 线性化测量函数:$z_t=h(x_t)+{\delta }_t$
  • 对于高斯分布，${\bar{\mu }}_t$是状态变量$x_t$最可能的状态
  • 所以对$z_t=h(x_t)+{\delta }_t$在${\bar{\mu }}_t$处展开(即$a={\bar{\mu }}_t$)
    $$\begin{gathered} h(x_t)\approx h({\bar{\mu }}_t)+h^{\prime }({\bar{\mu }}_t)(x_t-{\bar{\mu }}_t) \\ \downarrow \text{定义：}{H}_t:=h^{\prime }({\bar{\mu }}_t)=\frac{\partial h(x_t)}{\partial x_t}{|}_{x_t={\bar{\mu }}_t}\text{雅克比矩阵} \\ \downarrow \\ {z}_t\approx h({\bar{\mu }}_t)+H_t(x_t-{\bar{\mu }}_t)+{\delta }_t \\ \downarrow \\ {z}_t\sim N(z_t;h({\bar{\mu }}_t)+H_t(x_t-{\bar{\mu }}_t),Q_t) \\ \downarrow \\ \begin{array}{rl}& p(z_t|x_t)\\ & =det(2\pi Q_t{)}^{-\frac{1}{2}}\exp \left\{-\frac{1}{2}(z_t-h({\bar{\mu }}_t)-H_t(x_t-{\bar{\mu }}_t){)}^T{Q}_t^{-1}(z_t-h({\bar{\mu }}_t)-H_t(x_t-{\bar{\mu }}_t))\right\}\end{array} \\ \text{雅克比矩阵：}{H}_t=h^{\prime }({\bar{\mu }}_t)={\left[\begin{array}{cccc}\frac{\partial h_1}{\partial x_1}& \frac{\partial h_1}{\partial x_2}& \cdots & \frac{\partial h_1}{\partial x_n}\\ \frac{\partial h_2}{\partial x_1}& \frac{\partial h_2}{\partial x_2}& \cdots & \frac{\partial h_2}{\partial x_n}\\ ⋮& ⋮& ⋮& ⋮\\ \frac{\partial h_n}{\partial x_1}& \frac{\partial h_n}{\partial x_2}& \cdots & \frac{\partial h_n}{\partial x_n}\end{array}\right]}_{x_t={\bar{\mu }}_t} \end{gathered}$$

3 扩展卡尔曼滤波算法（EKF）

• 通过上一部分对状态转移函数和观测函数的线性化，使得这两个函数用两个线性函数表示，使得置信度$bel(x_t)$还服从正态分布
• 这里先给出扩展卡尔曼滤波算法，并与卡尔曼滤波进行对比，具体公式推导见下面
• • 扩展卡尔曼滤波算法

• • 卡尔曼滤波算法

• 将扩展卡尔曼滤波与卡尔曼滤波进行一下对比：
  1. $A_t{\mu }_{t-1}+B_t{u}_t\Rightarrow g(u_t,{\mu }_{t-1})$
  2. $C_t\overline{{\mu }_t}\Rightarrow h(\overline{{\mu }_t})$
  3. 雅可比矩阵$G_t$代替$A_t,B_t$
  4. 雅可比矩阵$H_t$代替$C_t$

4 扩展卡尔曼滤波实例

• 这里采用滤波算法 “扩展卡尔曼滤波（EFK）实例” 讲解的例子
• 实例：雷达监测空中抛物轨迹
  • 从空中位置$(x(0)=0,y(0)=500)$水平抛射出一个物体（初始水平速度为$v_x(0)=50$，初始竖直速度为$v_y(0)=0$）
  • 物体受重力$g=9.8$和阻尼力（与速度的平方成正比）的影响
  • 水平和竖直阻尼系数分别为$k_x=0.01,k_y=0.05$，不确定度为零均值白噪声$\delta a_x\sim N(0,0.09),\delta a_y\sim N(0,0.09)$
  • 在坐标原点处有一雷达，可测得距离$r$,角度$\alpha$, 不确定度为零均值白噪声$\delta r\sim N(0,64),\delta \alpha \sim N(0,0.01)$

• 状态变量: 物体横向位置$x(k)$，物体横向速度$v_x(k)$，物体纵向位置$y(k)$，物体纵向速度$v_y(k)$
  $$X(k)=[x(k),v_x(k),y(k),v_y(k)]$$

• 状态方程：
  $$\begin{array}{rl}x(k+1)& =x(k)+v_x(k)\cdot T\\ v_x(k+1)& =v_x(k)-(k_x\cdot v_x^2(k)+\delta a_x)\cdot T\\ y(k+1)& =y(k)+v_y(k)\cdot T\\ v_y(k+1)& =v_y(k)+(k_y\cdot v_y^2(k)-g+\delta a_y)\cdot T\end{array}$$

• 观测方程：
  $$\begin{array}{rl}r(k)& =\sqrt{x(k{)}^2+y(k{)}^2}+\delta r\\ \alpha (k)& =\arctan \frac{x(k)}{y(k)}+\delta \alpha \end{array}$$

• $\text{雅克比矩阵：}{G}_t$
  $$G_t={\left[\begin{array}{cccc}1& T& 0& 0\\ 0& 1-2k_x{v}_{x}T& 0& 0\\ 0& 0& 1& T\\ 0& 0& 0& 1+2k_y{v}_{y}T\end{array}\right]}_{x_{t-1}={\mu }_{t-1}}$$

• $\text{雅克比矩阵：}{H}_t$
  $$H_t={\left[\begin{array}{cccc}\frac{x}{r}& 0& \frac{y}{r}& 0\\ \frac{1/y}{1+(x/y{)}^2}& 0& \frac{-x/y^2}{1+(x/y{)}^2}& 0\end{array}\right]}_{x_t={\bar{\mu }}_t}$$

• 仿真时间总为$t=15$，采样周期$T=0.1$, 滤波效果如图：

• matlab 代码

clear;
close all;
clc
%% 初值设定
x_0 = 0;                    %初始x位置
y_0 = 500;                  %初始y位置
v_x_0 = 50;                 %初始x速度
v_y_0 = 0;                  %初始y速度
g = 9.8;                    % 重力加速度
k_x = 0.01;                 % 阻尼系数
k_y = 0.05;                 % 阻尼系数
sigma_a_x = 0.09;           % 状态转移不确定度方差
sigma_a_y = 0.09;           % 状态转移不确定度方差
sigma_r = 64;               % 测量不确定度方差
sigma_alpha = 0.01;         % 测量不确定度方差
t = 15;                     % 仿真时间
T = 0.1;                    % 采样周期 
len = fix(t/T);             % 仿真步数
%% 真实轨迹
X = zeros(len, 4);
X(1,:) = [x_0, v_x_0, y_0, v_y_0];
for k = 2 : len
    x_k = X(k-1,1);
    v_x_k = X(k-1,2);
    y_k = X(k-1,3);
    v_y_k = X(k-1,4);
    x_k = x_k + v_x_k * T;
    v_x_k = v_x_k - (k_x*v_x_k^2 + sqrt(sigma_a_x)*randn(1,1)) * T;
    y_k = y_k + v_y_k * T;
    v_y_k = v_y_k + (k_y*v_y_k^2 - g + sqrt(sigma_a_y)*randn(1,1)) * T;   
    X(k,:) = [x_k, v_x_k, y_k, v_y_k];
end
X_temp = X;
%% 雷达测量
Z = zeros(len, 2);
for k = 1 : len
    x_k = X(k,1);
    y_k = X(k,3);
    r = sqrt(x_k^2 + y_k^2) + sqrt(sigma_r)*randn(1,1);
    alpha = atan(x_k / y_k) * 180 / pi + sqrt(sigma_alpha)*randn(1,1);
    Z(k,:) = [r, alpha];
end
%% EKF 扩展卡尔曼滤波
R_k = diag([0; sigma_a_x; 0; sigma_a_y]);	% 状态转移误差的协方差矩阵
Q_k = diag([sigma_r, sigma_alpha]);         % 测量函数误差的协方差矩阵
sigma_k = 10 * eye(4);                      % 最优状态协方差矩阵
sigma_bar_k= 10 * eye(4);                   % 预测状态协方差矩阵
mu_k = [0, 40, 400, 0]';                    % 最优状态均值矩阵
mu_bar_k = zeros(4,1);                      % 预测的状态均值矩阵
z_k = zeros(4,1);                           % 观测矩阵
X_est = zeros(len,4);                       %EKF后的状态存储
for k = 1 : len
    % 1 状态预测
    x1 = mu_k(1) + mu_k(2)*T;
    v_x1 = mu_k(2) - (k_x*mu_k(2)^2)*T;
    y1 = mu_k(3) + mu_k(4)*T;
    v_y1 = mu_k(4) + (k_y*mu_k(4)^2 - g)*T;
    mu_bar_k = [x1; v_x1; y1; v_y1];            % 预测的均值   
    G_k = zeros(4,4);                       % 状态雅可比矩阵
    G_k(1,1) = 1; G_k(1,2) = T;
    G_k(2,2) = 1 - 2*k_x*v_x1*T;
    G_k(3,3) = 1; G_k(3,4) = T;
    G_k(4,4) = 1 + 2*k_y*v_y1*T;    
    sigma_bar_k = G_k * sigma_k * G_k' + R_k;
    % 2 观测更新
    r = sqrt(x1*x1+y1*y1);
    alpha = atan(x1/y1)*180/pi;
    z_k = [r, alpha]';                      % h(\bar{\mu}_t) 
    H_k = zeros(2,4);                       % 状态雅可比矩阵
    x = mu_k(1); y = mu_k(3); 
    H_k(1,1) = x / r;   H_k(1,3) = y / r;
    H_k(2,1) = (1/y)/(1 + (x/y)^2);   H_k(2,3) = (-x/(y^2))/(1 + (x/y)^2);
    K_k = sigma_bar_k * H_k' * (H_k * sigma_bar_k * H_k' + Q_k)^(-1);
    mu_k = mu_bar_k + K_k * (Z(k,:)' - z_k);
    sigma_k = (eye(4) - K_k * H_k) * sigma_bar_k;
    % 3 存储EKF后的值
    X_est(k,:) = mu_k;
end
%% 绘图 
figure, hold on, grid on;
plot(X(:,1),X(:,3),'-g');                                       %真实位置
plot(Z(:,1).*sin(Z(:,2)*pi/180), Z(:,1).*cos(Z(:,2)*pi/180),'-b');   %观测位置
plot(X_est(:,1),X_est(:,3), 'r');                               %最优位置
xlabel('X'); 
ylabel('Y'); 
title('EKF simulation');
legend('real', 'measurement', 'ekf estimated');
axis([-5,230,290,530]);

5 扩展卡尔曼滤波（EKF）公式推导

• 这里的公式推导用卡尔曼滤波公式进行类比，详细过程可以结合卡尔曼滤波公式的推导过程
  $$\begin{gathered} \text{Step 1: 预测}\{\begin{array}{c}{\bar{\mu }}_t=g(u_t,{\mu }_{t-1})\\ {\bar{\Sigma }}_t=G_t{\Sigma }_{t-1}{G}_t^T+R_t\end{array} \\ \text{Step 2: 测量更新}\{\begin{array}{c}{K}_t={\bar{\Sigma }}_t{H}_t^T(H_t{\bar{\Sigma }}_t{H}_t^T+Q_t{)}^{-1}\\ {\mu }_t={\bar{\mu }}_t+K_t(z_t-h({\bar{\mu }}_t))\\ {\Sigma }_t=(I-K_t{H}_t){\bar{\Sigma }}_t\end{array} \end{gathered}$$

5.1 预测公式推导

• 预测公式：
  $$\begin{gathered} \overline{bel}(x_t)=\int p(x_t|x_{t-1},u_t)bel(x_{t-1})d{x}_{t-1} \\ \text{卡尔曼滤波：}p(x_t|x_{t-1},u_t)\sim N(x_t;A_t{x}_{t-1}+B_t{u}_t,R_t) \\ bel(x_{t-1})\sim N(x_{t-1};u_{t-1},{\Sigma }_{t-1}) \\ \text{扩展卡尔曼滤波：}p(x_t|x_{t-1},u_t)\sim N(x_t;g(u_t,{\mu }_{t-1})+G_t(x_{t-1}-{\mu }_{t-1}),R_t) \\ bel(x_{t-1})\sim N(x_{t-1};u_{t-1},{\Sigma }_{t-1}) \end{gathered}$$
• 不同之处对比：$A_t{x}_{t-1}+B_t{u}_t\iff g(u_t,{\mu }_{t-1})+G_t(x_{t-1}-{\mu }_{t-1})$
  • $A_t\iff G_t$
  • $B_t{u}_t\iff g(u_t,{\mu }_{t-1})-G_t{\mu }_{t-1}$
• 那么扩展卡尔曼滤波预测步的公式可以根据卡尔曼滤波公式进行类比
  $$\begin{gathered} \text{Step 1: 预测}\{\begin{array}{c}{\bar{\mu }}_t=A_t{\mu }_{t-1}+B_t{u}_t\Rightarrow G_t{\mu }_{t-1}+g(u_t,{\mu }_{t-1})-G_t{\mu }_{t-1}=g(u_t,{\mu }_{t-1})\\ {\bar{\Sigma }}_t=A_t{\Sigma }_{t-1}{A}_t^T+R_t\Rightarrow G_t{\Sigma }_{t-1}{G}_t^T+R_t\end{array} \\ \downarrow \\ \text{Step 1: 预测}\{\begin{array}{c}{\bar{\mu }}_t=g(u_t,{\mu }_{t-1})\\ {\bar{\Sigma }}_t=G_t{\Sigma }_{t-1}{G}_t^T+R_t\end{array} \end{gathered}$$

5.2 测量更新公式推导

• 测量更新公式：
  $$\begin{gathered} bel(x_t)=\eta p(z_t|x_t)\overline{bel}(x_t)(\eta :\text{用于归一化}) \\ \text{卡尔曼滤波：}p(z_t|x_t)\sim N(z_t;C_t{x}_t,Q_t) \\ \overline{bel}(x_t)\sim N(x_t;{\bar{\mu }}_t,{\bar{\Sigma }}_t) \\ \text{扩展卡尔曼滤波：}p(x_t|x_{t-1},u_t)\sim N(z_t;h({\bar{\mu }}_t)+H_t(x_t-{\bar{\mu }}_t),Q_t) \\ \overline{bel}(x_t)\sim N(x_t;{\bar{\mu }}_t,{\bar{\Sigma }}_t) \end{gathered}$$
• 不同之处对比：$C_t{x}_t\iff h({\bar{\mu }}_t)+H_t(x_t-{\bar{\mu }}_t)$
  • $C_t\iff H_t$
  • $C_t{\bar{\mu }}_t\iff h({\bar{\mu }}_t)+H_t({\bar{\mu }}_t-{\bar{\mu }}_t)=h({\bar{\mu }}_t)$
• 那么扩展卡尔曼滤波预测步的公式可以根据卡尔曼滤波公式进行类比
  $$\begin{gathered} \text{Step 2: 测量更新}\{\begin{array}{c}{K}_t={\bar{\Sigma }}_t{C}_t^T(C_t{\bar{\Sigma }}_t{C}_t^T+Q_t{)}^{-1}\Rightarrow {\bar{\Sigma }}_t{H}_t^T(H_t{\bar{\Sigma }}_t{H}_t^T+Q_t{)}^{-1}\\ {\mu }_t={\bar{\mu }}_t+K_t(z_t-C_t{\bar{\mu }}_t)\Rightarrow {\bar{\mu }}_t+K_t(z_t-h({\bar{\mu }}_t))\\ {\Sigma }_t=(I-K_t{C}_t){\bar{\Sigma }}_t\Rightarrow (I-K_t{H}_t){\bar{\Sigma }}_t\end{array} \\ \downarrow \\ \text{Step 2: 测量更新}\{\begin{array}{c}{K}_t={\bar{\Sigma }}_t{H}_t^T(H_t{\bar{\Sigma }}_t{H}_t^T+Q_t{)}^{-1}\\ {\mu }_t={\bar{\mu }}_t+K_t(z_t-h({\bar{\mu }}_t))\\ {\Sigma }_t=(I-K_t{H}_t){\bar{\Sigma }}_t\end{array} \end{gathered}$$

6 扩展卡尔曼滤波的优缺点

• 优点：计算简单效率高
• 缺点：对状态转移函数和测量函数的线性近似，使得结果存在误差
• • 原函数不确定度（方差）越大，近似效果越差，见下图：
    1. 右下角$p(x)$的分布均值相同，方差不同
    2. 右上角变换函数$y=g(x)$相同
    3. 左上角$p(y)$的分布，下图误差小于上图，原因在于下图的方差较小

• • 原函数局部非线性越强，近似效果越差，见下图：
    1. 右下角$p(x)$的分布均值不同，方差相同
    2. 右上角变换函数$y=g(x)$相同
    3. 左上角$p(y)$的分布，下图误差小于上图，原因在于下图变换函数的局部非线性越弱

• 总结：状态变量的不确定度（方差）对EKF影响很大，尽量保持一个小的不确定度

7 参考文献

• Thrun, & Sebastian. (2005). Probabilistic robotics.
• 滤波算法 “扩展卡尔曼滤波（EFK）实例” 讲解