1 前言
-
公式推导也采用和之前一样的类比方法
-
传统的高斯分布用均值 μ \mu μ和方差 Σ \Sigma Σ表示,而信息滤波的高斯分布用信息向量 ξ \xi ξ和信息矩阵 Ω \Omega Ω表示, 详细介绍见信息滤波
Ω = Σ − 1 ξ = Σ − 1 μ ⇒ Σ = Ω − 1 μ = Ω − 1 ξ 注:方差信息矩阵 Σ 也叫做不确定度矩阵;信息矩阵 Ω 也叫做精确度矩阵 \begin{aligned} \Omega &= \Sigma^{-1}\\ \xi &= \Sigma^{-1} \mu \end{aligned} \Rightarrow \begin{aligned} \Sigma &= \Omega^{-1}\\ \mu&= \Omega^{-1} \xi \end{aligned}\\ \text{注:方差信息矩阵$\Sigma$也叫做不确定度矩阵;信息矩阵$\Omega$也叫做精确度矩阵} Ωξ=Σ−1=Σ−1μ⇒Σμ=Ω−1=Ω−1ξ注:方差信息矩阵Σ也叫做不确定度矩阵;信息矩阵Ω也叫做精确度矩阵 -
扩展信息滤波和扩展卡尔曼滤波一样,都是用在非线性状态转移函数和非线性测量函数中
x t = g ( u t , x t − 1 ) + ϵ t z t = h ( x t ) + δ t x_t = g(u_t, x_{t-1}) + \epsilon_t\\ z_t = h(x_t) + \delta_t xt=g(ut,xt−1)+ϵtzt=h(xt)+δt
2 扩展信息滤波算法
-
扩展信息滤波算法
-
从算法中可以看出和前面介绍的几种滤波方法类似:均由预测和测量更新组成
3 实例
- 这里采用扩展卡尔曼滤波的实例:
- 实例:雷达监测空中抛物轨迹
- 从空中位置 ( x ( 0 ) = 0 , y ( 0 ) = 500 ) (x(0)=0,y(0)=500) (x(0)=0,y(0)=500)水平抛射出一个物体(初始水平速度为 v x ( 0 ) = 50 v_x(0)=50 vx(0)=50,初始竖直速度为 v y ( 0 ) = 0 v_y(0)=0 vy(0)=0)
- 物体受重力 g = 9.8 g=9.8 g=9.8和阻尼力(与速度的平方成正比)的影响
- 水平和竖直阻尼系数分别为 k x = 0.01 , k y = 0.05 k_x=0.01,k_y=0.05 kx=0.01,ky=0.05,不确定度为零均值白噪声 δ a x ∼ N ( 0 , 0.09 ) ∼ N ( ξ = 0 , Ω = 1 / 0.09 ) , δ a y ∼ N ( 0 , 0.09 ) ∼ N ( ξ = 0 , Ω = 1 / 0.09 ) \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) δax∼N(0,0.09)∼N(ξ=0,Ω=1/0.09),δay∼N(0,0.09)∼N(ξ=0,Ω=1/0.09)
- 在坐标原点处有一雷达,可测得距离 r r r,角度 α \alpha α, 不确定度为零均值白噪声 δ r ∼ N ( 0 , 64 ) ∼ N ( ξ = 0 , Ω = 1 / 64 ) , δ α ∼ N ( 0 , 0.01 ) ∼ N ( ξ = 0 , Ω = 1 / 0.01 ) \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) δr∼N(0,64)∼N(ξ=0,Ω=1/64),δα∼N(0,0.01)∼N(ξ=0,Ω=1/0.01)
-
状态变量: 物体横向位置 x ( k ) x(k) x(k),物体横向速度 v x ( k ) v_x(k) vx(k),物体纵向位置 y ( k ) y(k) y(k),物体纵向速度 v y ( k ) v_y(k) vy(k)
X ( k ) = [ x ( k ) , v x ( k ) , y ( k ) , v y ( k ) ] X(k) = [x(k), v_x(k), y(k), v_y(k)] X(k)=[x(k),vx(k),y(k),vy(k)] -
状态方程:
x ( k + 1 ) = x ( k ) + v x ( k ) ⋅ T v x ( k + 1 ) = v x ( k ) − ( k x ⋅ v x 2 ( k ) + δ a x ) ⋅ T y ( k + 1 ) = y ( k ) + v y ( k ) ⋅ T v y ( k + 1 ) = v y ( k ) + ( k y ⋅ v y 2 ( k ) − g + δ a y ) ⋅ T \begin{aligned} 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{aligned} x(k+1)vx(k+1)y(k+1)vy(k+1)=x(k)+vx(k)⋅T=vx(k)−(kx⋅vx2(k)+δax)⋅T=y(k)+vy(k)⋅T=vy(k)+(ky⋅vy2(k)−g+δay)⋅T -
观测方程:
r ( k ) = x ( k ) 2 + y ( k ) 2 + δ r α ( k ) = arctan x ( k ) y ( k ) + δ α \begin{aligned} r(k) &= \sqrt{x(k)^2 + y(k)^2 } + \delta r\\ \alpha(k) &= \arctan{\frac{x(k)}{y(k)}} + \delta \alpha\\ \end{aligned} r(k)α(k)=x(k)2+y(k)2+δr=arctany(k)x(k)+δα -
雅克比矩阵: G t \text{雅克比矩阵:} G_t 雅克比矩阵:Gt
G t = [ 1 T 0 0 0 1 − 2 k x v x T 0 0 0 0 1 T 0 0 0 1 + 2 k y v y T ] x t − 1 = μ t − 1 G_t = \begin{bmatrix} 1& T& 0& 0 \\ 0& 1 - 2k_x v_xT& 0& 0\\ 0& 0& 1& T \\ 0& 0& 0& 1 + 2k_y v_yT\\ \end{bmatrix}_{x_{t-1}=\mu_{t-1}} Gt=⎣⎢⎢⎡1000T1−2kxvxT00001000T1+2kyvyT⎦⎥⎥⎤xt−1=μt−1 -
雅克比矩阵: H t \text{雅克比矩阵:} H_t 雅克比矩阵:Ht
H t = [ x r 0 y r 0 1 / y 1 + ( x / y ) 2 0 − x / y 2 1 + ( x / y ) 2 0 ] x t = μ ˉ t H_t = \begin{bmatrix} \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{bmatrix}_{x_t = \bar{\mu}_{t}} Ht=[rx1+(x/y)21/y00ry1+(x/y)2−x/y200]xt=μˉt -
仿真时间总为 t = 15 t=15 t=15,采样周期 T = 0.1 T=0.1 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