拓展卡尔曼滤波

南京航空航天大学

随机信号小论文题目扩展卡尔曼滤波

学生姓名梅晟

学号SX1504059

学院电子信息工程学院

专业通信与信息系统

扩展卡尔曼滤波

一、引言

20世纪60年代，在航空航天工程突飞猛进而电子计算机又方兴未艾之时，卡尔曼发表了论文《A New Approach to Linear Filtering and Prediction Problems》（一种关于线性滤波与预测问题的新方法），这让卡尔曼滤波成为了时域内有效的滤波方法，从此各种基于卡尔曼滤波的方法横空出世，在目标跟踪、故障诊断、计量经济学、惯导系统等方面得到了长足的发展。

二、卡尔曼滤波器

卡尔曼滤波是一种高效率的递归滤波器(自回归滤波器), 它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。

卡尔曼在NASA埃姆斯研究中心访问时，发现他的方法对于解决阿波罗计划的轨道预测很有用，后来阿波罗飞船的导航电脑便使用了这种滤波器。目前,卡尔曼滤波已经有很多不同的实现。卡尔曼最初提出的形式现在一般称为简单卡尔曼滤波器。除此以外，还有施密特扩展滤波器、信息滤波器以及很多Bierman, Thornton 开发的平方根滤波器的变种。也许最常见的卡尔曼滤波器是锁相环，它在收音机、计算机和几乎任何视频或通讯设备中广泛存在。

三、扩展卡尔曼滤波器

3.1 被估计的过程信号

卡尔曼最初提出的滤波理论只适用于线性系统,Bucy，Sunahara等人提出并研究了扩展卡尔曼滤波(Extended Kalman Filter，简称EKF)，将卡尔曼滤波理论进一步应用到非线性领域。EKF的基本思想是将非线性系统线性化，然后进行卡尔曼滤波，因此EKF是一种次优滤波。

同泰勒级数类似，面对非线性关系时，我们可以通过求过程和量测方程的偏导来线性化并计算当前估计。假设过程具有状态向量x∈?n，其状态方程为非线性随机差分方程的形式。

x k=f x k?1,u k?1,w k?1(1.1)

观测变量z∈?m为：

z k=?(x k,v k)(1.2)

随机变量w k和v k代表过程激励噪声和观测噪声。它们为相互独立，服从正态分布的白色噪声：

p(w) ～N(0, Q),

p(v) ～N(0, R).

实际系统中，过程激励噪声协方差矩阵Q 和观测噪声协方差矩阵R 可能会随每次迭代计算而变化。但在这我们假设它们是常数。

差分方程式(1.1)中的非线性函数f 将过去k ?1 时刻状态与现在k 时刻状态联系起来。量测方程(1.2)中的驱动函数u k和零均值过程噪声w k是它的参数。非线性函数h 反映了状态变量x k和观测变量z k的关系。实际中我们显然不知道每一时刻噪声w k和v k各自的值。但是，我们可以将它们假设为零，从而估计状态向量和观测向量为：

x k=f x k?1,u k?1,0(1.3)

和

z k=?(x k,0)(1.4)

其中，x k是过程相对前一时刻k 的后验估计。

有一点非常重要，那就是扩展卡尔曼滤波器的一个基本缺陷：离散随机变量的分布（或连续随机变量的密度）在经过非线性系统转化后不再是正态的了。扩展卡尔曼滤波器其实就是一个通过线性化而达到渐进最优贝叶斯决策的特殊状态估计器。

3.2 滤波器的计算原型

为了估计一个具有非线性差分和量测关系的过程，我们先给出式(1.3)和

式(1.4)的一个新的线性化表示：

x k≈x k+A x k?1?x k?1+Ww k?1(1.5)

z k≈z k+H x k?x k+Vv k(1.6)

其中，

? x k和z k是状态向量和观测向量的真值，

? x k和z k来自1.3式和1.4式，是状态向量和观测向量的近似值，

? x k是k 时刻状态向量的后验估计，

?随机变量w k和v k表示过程激励噪声和观测噪声。

? A 是f 对x 的偏导的雅可比矩阵：

A[i,j]=ef i

j

x k?1,u k?1,0

? W 是f 对w 的偏导的雅可比矩阵：

W[i,j]=ef i

ew j

x k?1,u k?1,0

? H 是h 对x 的偏导的雅可比矩阵：

H[i,j]=e?i

ex j

x k,0

? V 是h 对v 的偏导的雅可比矩阵：

V[i,j]=e?i

j

x k,0

现在我们定义一个新的预测误差的表达式：

e x

k

≡x k?x k(1.7)

和观测变量的残余，

e z

k

≡z k?z k(1.8)

但实际中无法获得(1.7)式中的x k,它是状态向量的真值，也就是要估计的对象。同样，(1.8)式中的z k也是无法获取的，它是用来估计x k的观测向量的真值。由

(1.7)式和(1.8)式我们可以写出误差过程的表达式：

e x

k

≈A x k?1?x k?1+?k(1.9)

e z

k ≈He x

k

+ηk(1.10)

?k和ηk代表具有零均值和协方差矩阵WQW T和VRV T的独立随机变量，Q 和R 分别为过程激励噪声协方差矩阵和观测噪声协方差矩阵。在此我们利用(1.8)

式中的观测残余真值e z

k 去估计(1.9)式中的预测误差e x

k

，估计结果记为e k，结合

(1.7)式可以获得初始非线性过程的后验状态估计：

x k=x k+e k(1.11)

(1.9)式和(1.10)式中的随机变量具有如下概率分布:

p(e x

k )~N(0,E[e x

k

e x

k

T])

p(?k)~N(0,WQ k W T)

p(ηk)~N(0,VR k V T)

令e k的估计值为零，由以上近似，可以写出估计e k的卡尔曼滤波器表达式：

e k=K k e z

k

(1.12)

将(1.8)式和(1.12)式代入(1.11)式，得到：

x k=x k+K k e z

k

=x k+K k(z k?z k)(1.13) 3.3 拓展卡尔曼滤波总结

扩展卡尔曼滤波器的一个重要特性是卡尔曼增益K k的表达式中的雅可比矩阵H k能够正确地传递或“加权”观测信息中的有用部分。例如，如果

通过h 观测变量z k和状态变量没有一一对应的关系，雅可比矩阵H k便通

过改变卡尔曼增益从而使得残余z k??(x k?,0)中真正作用于状态变量的部分被加权。当然，如果整个观测中观测变量z k和状态变量通过h 都没有一个一一对应的关系，那么滤波器很快就会发散。这种情况下过程是不可观测的。

拓展卡尔曼滤波器基本运行流程图如下：

四、Matlab仿真

附程序如下:

clc; close all; clear all;

Xint_v = [1; 0; 0; 0; 0];

wk = [1 0 0 0 0];

vk = [1 0 0 0 0];

for ii = 1:1:length(Xint_v)

Ap(ii) = Xint_v(ii)*2;

W(ii) = 0;

H(ii) = -sin(Xint_v(ii));

V(ii) = 0;

Wk(ii) = 0;

end

Uk = randn(1,200);

Qu = cov(Uk);

Vk = randn(1,200);

Qv = cov(Vk);

C = [1 0 0 0 0];

n = 100;

[YY XX] = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);

for it = 1:1:length(XX)

MSE(it) = YY(it) - XX(it);

end

tt = 1:1:length(XX);

figure(1); subplot(211); plot(XX); title('ORIGINAL SIGNAL');

subplot(212); plot(YY); title('ESTIMATED SIGNAL');

figure(2); plot(tt,XX,tt,YY); title('Combined plot'); legend('original','estimated'); figure(3); plot(MSE.^2); title('Mean square error');

%--------------------------------------------------------------------

%--------------------------------------------------------------------

function [YY,XX] = EKLMNFTR1(Ap,Xint_v,Uk,Qu,Vk,Qv,C,n,Wk,W,V);

Ap(2,:) = 0;

for ii = 1:1:length(Ap)-1

Ap(ii+1,ii) = 1;

end

inx = 1;

UUk = [Uk(inx); 0; 0; 0; 0];

PPk = (Xint_v*Xint_v');

VVk = [Vk(inx); 0; 0; 0; 0];

Qv = V*V';

for ii = 1:1:length(Xint_v)

XKk(ii,1) = Xint_v(ii)^2; %FIRST STEP end

PPk = Ap*PPk*Ap'; % SECOND STEP

Kk = PPk*C'*inv( (C*PPk*C') + (V*Qv*V') ); % THIRD STEP

for ii = 1:1:length(Xint_v)

XUPK(ii,1) = XKk(ii)^2 + UUk(ii); % UPPER EQUATIONS.

Zk(ii,1) = cos(XUPK(ii)) + VVk(ii); % UPPER EQUATIONS.

end

for ii = 1:1:length(XKk)

XBARk(ii,1)=XKk(ii)+ Kk(ii)*(Zk(ii) - (cos(XKk(ii)))) ; % FOURTH STEP

end

II = eye(5,5);

Pk = ( II - Kk*C)*PPk; % FIFTH STEP %--------------------------------------------------------------------

%--------------------------------------------------------------------

for ii = 1:1:n

UUk = [Uk(ii+1); 0; 0; 0; 0];

PPk = XBARk*XBARk';

VVk = [Vk(ii+1); 0; 0; 0; 0];

XKk = exp(-XBARk); % FIRST STEP PPkM = Ap*PPk*Ap'; % SECOND STEP

Kk = PPkM*C'*inv( (C*PPkM*C') + (V*Qv*V') ); % THIRD STEP

fornn = 1:1:length(XBARk)

XUPK(nn) = exp(-XKk(nn)) + UUk(nn); % UPPER EQUATIONS. Zk(nn) = cos(XUPK(nn)) + VVk(nn); % UPPER EQUATIONS. end

for in = 1:1:length(XUPK)

XNEW(in) = XBARk(in)+Kk(in)*(Zk(in) - cos(XBARk(in))); % FOURTH STEP

end

II = eye(5,5);

Pk = (II - Kk*C)*PPkM; % FIFTH STEP

XBARk = XNEW;

OUTX(ii) = XBARk(1,1);

OUTY(ii) = Zk(1,1);

end

YY = OUTY;

XX = OUTX;