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