7 概率机器人 Probabilistic Robotics 扩展信息滤波算法

1 前言

• 从信息滤波到扩展信息滤波这里会类比从卡尔曼滤波到扩展卡尔曼滤波

• 公式推导也采用和之前一样的类比方法

• 传统的高斯分布用均值$\mu$和方差$\Sigma$表示，而信息滤波的高斯分布用信息向量$\xi$和信息矩阵$\Omega$表示， 详细介绍见信息滤波
  $$\begin{gathered} \begin{array}{rl}\Omega & ={\Sigma }^{-1}\\ \xi & ={\Sigma }^{-1}\mu \end{array}\Rightarrow \begin{array}{rl}\Sigma & ={\Omega }^{-1}\\ \mu & ={\Omega }^{-1}\xi \end{array} \\ \text{注：方差信息矩阵Σ也叫做不确定度矩阵；信息矩阵Ω也叫做精确度矩阵} \end{gathered}$$

• 扩展信息滤波和扩展卡尔曼滤波一样，都是用在非线性状态转移函数和非线性测量函数中
  $$\begin{gathered} x_t=g(u_t,x_{t-1})+{ϵ}_t \\ {z}_t=h(x_t)+{\delta }_t \end{gathered}$$

2 扩展信息滤波算法

• 扩展信息滤波算法
  

• 从算法中可以看出和前面介绍的几种滤波方法类似：均由预测和测量更新组成

3 实例

• 这里采用扩展卡尔曼滤波的实例：
• 实例：雷达监测空中抛物轨迹
  • 从空中位置$(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)\sim N(\xi =0,\Omega =1/0.09),\delta a_y\sim N(0,0.09)\sim N(\xi =0,\Omega =1/0.09)$
  • 在坐标原点处有一雷达，可测得距离$r$,角度$\alpha$, 不确定度为零均值白噪声$\delta r\sim N(0,64)\sim N(\xi =0,\Omega =1/64),\delta \alpha \sim N(0,0.01)\sim N(\xi =0,\Omega =1/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;                 % 阻尼系数
omega_a_x = 1 / 0.09;       % 状态转移不确定度方差
omega_a_y = 1 / 0.09;       % 状态转移不确定度方差
omega_r = 1 / 64;           % 测量不确定度方差
omega_alpha = 1 / 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(1 / omega_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(1 / omega_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(1 / omega_r)*randn(1,1);
    alpha = atan(x_k / y_k) * 180 / pi + sqrt(1 / omega_alpha)*randn(1,1);
    Z(k,:) = [r, alpha];
end
%% EIF 扩展信息滤波
R_k = diag([0; 1/omega_a_x; 0; 1/omega_a_y]);	% 状态转移误差的协方差矩阵
Q_k = diag([1 / omega_r, 1 / omega_alpha]);         % 测量函数误差的协方差矩阵
omega_k = 0.1 * eye(4);                     % 最优状态协方差矩阵
omega_bar_k= 0.1 * eye(4);                  % 预测状态协方差矩阵
xi_k = [0, 4, 40, 0]';                      % 最优状态均值矩阵
z_k = zeros(4,1);                           % 观测矩阵
X_est = zeros(len,4);                       %EKF后的状态存储
for k = 1 : len
    % 1 状态预测
    mu_k = omega_k^(-1) * xi_k;     
    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;
    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;
    omega_bar_k = (G_k * omega_k^(-1) * G_k' + R_k)^(-1);   
    mu_bar_k = [x1; v_x1; y1; v_y1];            % 预测的均值   
    xi_bar_k = omega_bar_k * mu_bar_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);
    omega_k = omega_bar_k + H_k'*(Q_k^(-1))*H_k;
    xi_k = xi_bar_k + H_k'*(Q_k^(-1))*(Z(k,:)' - z_k + H_k*mu_bar_k);
    % 3 存储EKF后的值
    X_est(k,:) = omega_k^(-1) * xi_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]);

4 公式推导

• 下面给出扩展卡尔曼滤波、扩展信息滤波主要公式：
  $$\begin{gathered} \text{扩展卡尔曼滤波：}\begin{array}{rl}& \text{Step 1: 预测}\{\begin{array}{cccc}& {\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}{cccc}& 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{array} \\ \Downarrow \\ \text{扩展信息滤波：}\begin{array}{rl}& \text{Step 1: 预测}\{\begin{array}{cccc}& {\mu }_{t-1}& =& {\Omega }_{t-1}^{-1}{\xi }_{t-1}\\ & {\bar{\Omega }}_t& =& (G_t{\Sigma }_{t-1}{G}_t^T+R_t{)}^{-1}\\ & {\bar{\xi }}_t& =& {\bar{\Omega }}_{t}g(u_t,{\mu }_{t-1})\end{array}\\ & \text{Step 2: 测量更新}\{\begin{array}{cccc}& {\bar{\mu }}_t& =& g(u_t,{\mu }_{t-1})\\ & {\Omega }_t& =& {\bar{\Omega }}_t+H_t^T{Q}_t^{-1}{H}_t\\ & {\xi }_t& =& {\bar{\xi }}_t+H_t^T{Q}_t^{-1}(z_t-h({\bar{\mu }}_t)+H_t{\bar{\mu }}_t)\end{array}\end{array} \end{gathered}$$
• 需要的替换公式：
  $$\begin{gathered} \begin{array}{rl}\Omega & ={\Sigma }^{-1}\\ \xi & ={\Sigma }^{-1}\mu \end{array}\iff \begin{array}{rl}\Sigma & ={\Omega }^{-1}\\ \mu & ={\Omega }^{-1}\xi \end{array} \\ \downarrow \\ \begin{array}{rl}{\bar{\Omega }}_t& ={\bar{\Sigma }}_t^{-1}\\ {\bar{\xi }}_t& ={\bar{\Sigma }}_t^{-1}{\bar{\mu }}_t\end{array}\iff \begin{array}{rl}{\bar{\Sigma }}_t& ={\bar{\Omega }}_t^{-1}\\ {\bar{\mu }}_t& ={\bar{\Omega }}_t^{-1}{\bar{\xi }}_t\end{array} \\ \begin{array}{rl}{\Omega }_{t-1}& ={\Sigma }_{t-1}^{-1}\\ {\xi }_{t-1}& ={\Sigma }_{t-1}^{-1}{\mu }_{t-1}\end{array}\iff \begin{array}{rl}{\Sigma }_{t-1}& ={\Omega }_{t-1}^{-1}\\ {\mu }_{t-1}& ={\Omega }_{t-1}^{-1}{\xi }_{t-1}\end{array} \\ \begin{array}{rl}{\Omega }_t& ={\Sigma }_t^{-1}\\ {\xi }_t& ={\Sigma }_t^{-1}{\mu }_t\end{array}\iff \begin{array}{rl}{\Sigma }_t& ={\Omega }_t^{-1}\\ {\mu }_t& ={\Omega }_t^{-1}{\xi }_t\end{array} \end{gathered}$$

4.1 预测公式推导

• line 2: 这个不用推导，替换公式里有
  $${\mu }_{t-1}={\Omega }_{t-1}^{-1}{\xi }_{t-1}$$
• line 3:
  $$\begin{gathered} {\bar{\Omega }}_t={\bar{\Sigma }}_t^{-1} \\ \downarrow \\ {\bar{\Omega }}_t=(G_t{\Sigma }_{t-1}{G}_t^T+R_t{)}^{-1} \\ \downarrow \\ {\bar{\Omega }}_t=(G_t{\Omega }_{t-1}^{-1}{G}_t^T+R_t{)}^{-1} \end{gathered}$$
• line 4:
  $$\begin{gathered} {\bar{\xi }}_t={\bar{\Sigma }}_t^{-1}{\bar{\mu }}_t \\ {\bar{\Sigma }}_t^{-1}={\bar{\Omega }}_t\downarrow {\bar{\mu }}_t=g(u_t,{\mu }_{t-1}) \\ {\bar{\xi }}_t={\bar{\Omega }}_{t}g(u_t,{\mu }_{t-1}) \end{gathered}$$
• 综上:
  $$\text{Step 1: 预测}\{\begin{array}{cccc}& {\mu }_{t-1}& =& {\Omega }_{t-1}^{-1}{\xi }_{t-1}\\ & {\bar{\Omega }}_t& =& (G_t{\Sigma }_{t-1}{G}_t^T+R_t{)}^{-1}\\ & {\bar{\xi }}_t& =& {\bar{\Omega }}_{t}g(u_t,{\mu }_{t-1})\end{array}$$

4.2 测量更新公式推导

• 在推导卡尔曼滤波时，得到与测量更新步骤等价的公式，类比出扩展卡尔曼滤波的等价公式，用这个等价的公式推导扩展信息滤波
  $$\begin{gathered} \{\begin{array}{cccc}& K_t& =& {\Sigma }_t{C}_t^T{Q}_t^{-1}\\ & {\mu }_t& =& {\bar{\mu }}_t+K_t(z_t-C_t{\bar{\mu }}_t)\\ & {\Sigma }_t& =& (C_t^T{Q}_t^{-1}{C}_t+{\bar{\Sigma }}_t^{-1}{)}^{-1}\end{array}\iff \{\begin{array}{cccc}& K_t& =& {\bar{\Sigma }}_t{C}_t^T(C_t{\bar{\Sigma }}_t{C}_t^T+Q_t{)}^{-1}\\ & {\mu }_t& =& {\bar{\mu }}_t+K_t(z_t-C_t{\bar{\mu }}_t)\\ & {\Sigma }_t& =& (I-K_t{C}_t){\bar{\Sigma }}_t\end{array} \\ \downarrow \\ \{\begin{array}{cccc}& K_t& =& {\Sigma }_t{H}_t^T{Q}_t^{-1}\\ & {\mu }_t& =& {\bar{\mu }}_t+K_t(z_t-h({\bar{\mu }}_t))\\ & {\Sigma }_t& =& (H_t^T{Q}_t^{-1}{H}_t+{\bar{\Sigma }}_t^{-1}{)}^{-1}\end{array}\iff \{\begin{array}{cccc}& 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}$$
• 用这个等价的公式推导扩展信息滤波
  $$\{\begin{array}{cccc}& K_t& =& {\Sigma }_t{H}_t^T{Q}_t^{-1}\\ & {\mu }_t& =& {\bar{\mu }}_t+K_t(z_t-h({\bar{\mu }}_t))\\ & {\Sigma }_t& =& (H_t^T{Q}_t^{-1}{H}_t+{\bar{\Sigma }}_t^{-1}{)}^{-1}\end{array}$$
• line 5: 在$x_{t-1}={\mu }_{t-1}$处线性化
  $${\bar{\mu }}_t=g(u_t,{\mu }_{t-1})$$
• line 6:
  $$\begin{gathered} {\Omega }_t={\Sigma }_t^{-1} \\ \downarrow {\Sigma }_t=(H_t^T{Q}_t^{-1}{H}_t+{\bar{\Sigma }}_t^{-1}{)}^{-1} \\ {\Omega }_t={\bar{\Omega }}_t+H_t^T{Q}_t^{-1}{H}_t \end{gathered}$$
• line 7:
  $$\begin{gathered} {\xi }_t={\Sigma }_t^{-1}{\mu }_t \\ \downarrow {\mu }_t={\bar{\mu }}_t+K_t(z_t-h({\bar{\mu }}_t)) \\ \downarrow {\mu }_t={\bar{\mu }}_t+{\Sigma }_t{H}_t^T{Q}_t^{-1}(z_t-h({\bar{\mu }}_t)) \\ {\xi }_t={\Sigma }_t^{-1}{\bar{\mu }}_t+H_t^T{Q}_t^{-1}(z_t-h({\bar{\mu }}_t)) \\ \downarrow {\Sigma }_t^{-1}=H_t^T{Q}_t^{-1}{H}_t+{\bar{\Sigma }}_t^{-1} \\ {\xi }_t=H_t^T{Q}_t^{-1}{H}_t{\bar{\mu }}_t+{\bar{\Sigma }}_t^{-1}{\bar{\mu }}_t+H_t^T{Q}_t^{-1}(z_t-h({\bar{\mu }}_t)) \\ \downarrow {\bar{\xi }}_t={\bar{\Sigma }}_t^{-1}{\bar{\mu }}_t \\ {\xi }_t={\bar{\xi }}_t+H_t^T{Q}_t^{-1}(z_t-h({\bar{\mu }}_t)+H_t{\bar{\mu }}_t) \end{gathered}$$
• 综上:
  $$\text{Step 2: 测量更新}\{\begin{array}{cccc}& {\bar{\mu }}_t& =& g(u_t,{\mu }_{t-1})\\ & {\Omega }_t& =& {\bar{\Omega }}_t+H_t^T{Q}_t^{-1}{H}_t\\ & {\xi }_t& =& {\bar{\xi }}_t+H_t^T{Q}_t^{-1}(z_t-h({\bar{\mu }}_t)+H_t{\bar{\mu }}_t)\end{array}$$

4.3 扩展信息滤波主要公式

$$\text{扩展信息滤波：}\begin{array}{rl}& \text{Step 1: 预测}\{\begin{array}{cccc}& {\mu }_{t-1}& =& {\Omega }_{t-1}^{-1}{\xi }_{t-1}\\ & {\bar{\Omega }}_t& =& (G_t{\Sigma }_{t-1}{G}_t^T+R_t{)}^{-1}\\ & {\bar{\xi }}_t& =& {\bar{\Omega }}_{t}g(u_t,{\mu }_{t-1})\end{array}\\ & \text{Step 2: 测量更新}\{\begin{array}{cccc}& {\bar{\mu }}_t& =& g(u_t,{\mu }_{t-1})\\ & {\Omega }_t& =& {\bar{\Omega }}_t+H_t^T{Q}_t^{-1}{H}_t\\ & {\xi }_t& =& {\bar{\xi }}_t+H_t^T{Q}_t^{-1}(z_t-h({\bar{\mu }}_t)+H_t{\bar{\mu }}_t)\end{array}\end{array}$$

5 参考文献

• Thrun, & Sebastian. (2005). Probabilistic robotics.