%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 卡尔曼滤波器在INS-GPS组合导航中应用仿真
% Author : lylogn
% Email : lylogn@gmail.com
% Company: BUAA-Dep3
% Time : 2013.01.06
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 参考文献:
% [1]. 邓正隆. 惯导技术, 哈尔滨工业大学粗仔出版社.2006.
clear all
%% 惯性-GPS组合导航模型参数初始化
we = 360/24/60/60*pi/180%地球自转角速度，弧度/s
psi = 10*pi/180 %psi角度 / 弧度
Tge = 0.12
Tgn = 0.10
Tgz = 0.10 %这三个参数的含义详见参考文献
sigma_ge=1
sigma_gn=1
sigma_gz=1
%% 连续空间系统状态方程
% X_dot(t) = A(t)*X(t) + B(t)*W(t)
A=[0 we*sin(psi) -we*cos(psi) 1 0 0 1 0 0
-we*sin(psi) 0 0 0 1 0 0 1 0
we*cos(psi) 0 0 0 0 1 0 0 1
00 0 -1/Tge 0 0 0 0 0
00 0 0 -1/Tgn 0 0 0 0
00 0 0 0 -1/Tgz 0 0 0
00 0 0 0 0 0 0 0
00 0 0 0 0 0 0 0
00 0 0 0 0 0 0 0]%状态转移矩阵
B=[00 0sigma_ge*sqrt(2/Tge) 0 0 0 0 0
00 00 sigma_gn*sqrt(2/Tgn) 0 0 0 0
00 00 0 sigma_gz*sqrt(2/Tgz) 0 0 0]'%输入控制矩阵
%% 转化为离散时间系统状态方程
% X(k+1) = F*X(k) + G*W(k)
T = 0.1
[F,G]=c2d(A,B,T)
H=[10 0 0 0 0 0 0 0
0 -sec(psi) 0 0 0 0 0 0 0]%观测矩稿纳阵
%% 卡尔曼滤波器参数初始化
t=0:T:50-T
length=size(t,2)
y=zeros(2,length)
Q=0.5^2*eye(3)%系统噪声协方差
R=0.25^2*eye(2) %测量噪声协方差
y(1,:)=2*sin(pi*t*0.5)
y(2,:)=2*cos(pi*t*0.5)
Z=y+sqrt(R)*randn(2,length) %生成的含有噪声的假定观测值，2维
X=zeros(9,length) %状态估计值，9维
X(:,1)=[0,0,0,0,0,0,0,0,0]' %状态估计初始值设定
P=eye(9) %状态估计协方差
%% 卡尔曼滤波算法迭代过程
for n=2:length
X(:,n)=F*X(:,n-1)
P=F*P*F'+ G*Q*G'
Kg=P*H'/(H*P*H'+R)
X(:,n)=X(:,n)+Kg*(Z(:,n)-H*X(:,n))
P=(eye(9,9)-Kg*H)*P
end
%% 绘图代码
figure(1)
plot(y(1,:))
hold on
plot(y(2,:))
hold off
title('理想的观测量')
figure(2)
plot(Z(1,:))
hold on
plot(Z(2,:))
hold off
title('带有噪声的观测量')
figure(3)
plot(X(1,:))
hold on
plot(X(2,:))
hold off
title('滤波后的观测量')
clearN=200%取200个数
w(1)=0
w=randn(1,N)%产生一个1×N的行向量，第一个数为0，w为过程噪声（其和后边的v在卡尔曼理论里均为高斯白噪声）
x(1)=0%状态x初始值
a=1%a为状态转移阵，此程序简单起见取1
for k=2:N
x(k)=a*x(k-1)+w(k-1) %系统状态方程，k时迅颤皮刻的状态等于k-1时刻状态乘以状态转移阵加噪声（此处忽略了系统的控制量）
end
V=randn(1,N)%测量噪声
q1=std(V)
Rvv=q1.^2
q2=std(x)
Rxx=q2.^2%此方程未用到Rxx
q3=std(w)
Rww=q3.^2%Rvv、Rww分别为过程噪声和测量洞山噪声的协方差(此方程只取一组数方差与协方差相同)
c=0.2
Y=c*x+V%量测方差，c为量测矩阵，同a简亩差化取为一个数
p(1)=0%初始最优化估计协方差
s(1)=0%s(1)表示为初始最优化估计
for t=2:N
p1(t)=a.^2*p(t-1)+Rww%p1为一步估计的协方差，此式从t-1时刻最优化估计s的协方差得到t-1时刻到t时刻一步估计的协方差
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv)%b为卡尔曼增益，其意义表示为状态误差的协方差与量测误差的协方差之比(个人见解)
s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1))%Y(t)-a*c*s(t-1)称之为新息，是观测值与一步估计得到的观测值之差，此式由上一时刻状态的最优化估计s(t-1)得到当前时刻的最优化估计s(t)
p(t)=p1(t)-c*b(t)*p1(t)%此式由一步估计的协方差得到此时刻最优化估计的协方差
end
t=1:N
plot(t,s,'r',t,Y,'g',t,x,'b')%作图，红色为卡尔曼滤波，绿色为量测，蓝色为状态
%整体来说，此卡尔曼程序就是一个循环迭代的过程，给出初始的状态x和协方差p，得到下一时刻的x和p，循环带入可得到一系列的最优的状态估计值，此方法通常用于目标跟踪和定位。
%本人研究方向与此有关，有兴趣可以交流下。
本文为离散卡尔曼滤波算法的一 一个简明教程，从算法思想、实现过程、理论推导和程序实现四个方面阐述和分析了卡尔曼滤波算法。
XU Ruilin完成本教程主要部分的编写，WANG Xuejun完成第3节的编写，ZHU Ximin完成2.2节的编写，WEN Shuhan完成2.3节的编写，MAO Bo完成全文整理、修订和排版。
卡尔曼滤波（Kalman Filtering）及其一系列的优化和改进算法是目前在求解运动状态推算问题上最为普遍和高效的方法。 鲁道夫·卡尔曼 （Rudolf Emil Kalman） 在NASA埃姆斯研究中心访问时，发现他的方侍让法适用于解决阿波罗计划的轨迹预测问题。阿波罗飞船的导航电脑就是使用这种滤波器进行轨迹预测。
卡尔曼滤波尤其适用于动态系统，这种方法对于内存要求极低而运算速度快，且能够保持较好的计算精度，这使得这种方法非常适合解决实时问题和应用于嵌入式系统，也就是说，卡尔曼滤波天然的适用于解决舰艇指控系统的航迹推算问题。在接下来的内容里，我们将逐步领会卡尔曼滤波的这些绝佳特点。
不过，现在我们先从复杂的舰艇航迹推算问题中解脱出来，从一个更加熟悉和简单的问题中来理解这个滤波算法的思想、过程和算法。
假设有一辆无人车WALL-E，需要导引它从A点到达B点，共有两种手段（ 图1 ）：
显然，两种方法都有一定的误差。如果单独采用某一种方法进行定位，WALL-E在误差的影响下将无法到达B点。因此，需要将两种方法结合起来，得到一个更加精确的结果，这就是卡耐茄尔曼滤波要解决的问题。
卡尔曼滤波方法如何看待我们的问题呢？在探究这个问题之前，我们先对问题进行抽象，并用数学语言来描述我们的问题。
我们用矢量 来描述WALL-E的运动状态，这个列矢量 包括位置矢量 和速度矢量 两个分量。在WALL-E的问题上，我们现在不知道位置 和速度 的准确值，但是知道WALL-E的运动模型满足 状态方程 ，定位的方法，也即观测WALL-E运动状态的方法满足 观测方程 . 当然，我们也知道，这两种方法都存在一定的误差 ，那么我们的问题就可以转化为一个优化问题——
在这一优化问题中，目标函数是要使预测（估计）误差最小，同时约束于估计方法 和 的条件下。在卡尔曼滤波中，我们的估计原则（也就是最小化估计误差的原则）是 最小方差无偏估计 [1] ，我们将通过后面的过程分析来说明这一点。
在我们正式开始引入公式分析卡尔昌谈察曼滤波问题之前，我们还必须解决一个问题------把连续的线性系统离散化，也就是将连续时域问题转化为时间序列问题。当然，目前我们只讨论线性系统的情况，关于非线性系统问题，我们有扩展卡尔曼滤波（Extended Kalman Filtering, EKF）和无迹卡尔曼滤波（Unscented Kalman Filtering, UKF）两种方法来求解。
补充内容------连续线性时变系统的离散化
设连续线性时变系统的时域状态方程为
若采样周期为 ，则从时刻 到时刻 ，有
令 ， ，则离散化的状态方程为
通过对线性系统的离散化处理，我们现在可以考虑每一个时刻WALL-E的运动状态。接下来，我们将用 来表示在 时刻运动状态的最优估计值；用 表示用 时刻对 时刻的状态预测值；用 表示对 时刻综合预测和观测两种方法的最优估计值。
在估计WALL-E位置的问题上，假定我们已经知道它是匀速直线运动，WALL-E身上还携带有一个GPS传感器可以提供它的位置信息，WALL-E在前进过程中可能会遇到一些情况，比如停止前进或是受到风的影响。
加入我们已知的是WALL-E上一个时刻的最佳估计状态，即k-1时刻的位置和速度，要求的是下一时刻即k时刻的最佳估计状态，即k时刻的位置和速度，我们可以发现有两种方法可以得到它的k时刻的状态：
一种是通过WALL-E设定程序计算得到下一秒的状态，比如现在设定是匀速直线运动，那么下一秒的速度应该是恒定不变的，而位置则是在上一秒位置的基础上加上时间乘以速度即一秒内走过的路程，但是现实生活中并不是理想的，机器人会受到摩擦力、风力等的影响，当然也可能会有顽皮的小孩挡住他前进的道路，这些因素使得WALL-E在k时的真实状态与我们计算得到的数据有所不同。
另一种是通过WALL-E所携带的GPS来确定它的位置，因为GPS是测量出的就是WALL-E的实时状态，因此它比较准确。但是GPS测量k时刻的状态有两个问题，一是GPS只能测出WALL-E的位置，而测不出它的速度；二是GPS传感器测量的时候也会有仪器的误差，只能说它是比较准确的，比较接近真实值的。
那么接下来问题来了，我们如何得到k时刻WALL-E的真实状态呢？
我们将第一种方法得到的状态值称为预测值，第二种方法得到的状态值称为测量值，对汽车的最佳估计就是将这两部分信息结合起来，尽量的去逼近k时刻的真实值。
下面再深入一些思考，怎么将这两部分结合起来？
在初始时间k-1， 是WALL-E的最佳估计值，WALL-E其实可以是估计值附近的任何位置，并且这种不确定性由该概率密度函数描述。WALL-E最有可能在这个分布的平均值附近。在下一个时间，估计的不确定性增加，用一个更大的方差表示，这是因为在时间步骤k-1和k之间，WALL-E可能收到了风力的影响，或者脚可能向前滑了一点，因此，它可能已经行进了与模型预测的距离不同的距离。
WALL-E位置的另一个信息来源来自测量，方差表示误差测量的不确定性，真正的位置同样可以是平均值附近的任何位置。
预测值和测量值，对WALL-E的最佳估计是将这两部分信息结合起来，将两个概率函数相乘得到另一个高斯函数，该估计值的方差小于先前估计值，并且该概率密度函数的平均值为我们提供了WALL-E位置的最佳估计。
以下，我们将进行e的运算推导
设：
则有实际目标变量的表达式：
数学模型中目标变量的表达式：
实际模型中测量变量的表达式：
数学模型中测量变量的表达式：
将目标变量的实际值和估计值相减：
将上述方程带入误差e的表达式，我们可得出误差e的解析解：
从推导结果中我们不难看出，估计值和实际值的误差随时间呈指数形式变化，当（F-KH）<1时，随着时间的推移，会无限趋近于零，也就是意味着估计值和实际值相吻合。这就是为什么卡尔曼滤波器可以完美预测出目标状态值的原理。
在估计WALL-E位置的问题上，我们不知道位置 和速度 的准确值，但是我们可以给出一个估计区间（ 图5.a ）。卡尔曼滤波假设所有的变量是随机的且符合高斯分布（正态分布）。每个变量有一个均值 和一个方差（ 图5.b ）。而 图5.c 则表示速度和位置是相关的。
假如我们已知上一个状态的位置值，现在要预测下一个状态的位置值。如果我们的速度值很高，我们移动的距离会远一点。相反，如果速度慢，WALL-E不会走的很远。这种关系在跟踪系统状态时很重要，它给了我们更多的信息：一个观测值告诉我们另一个观测值可能是什么样子。这就是卡尔曼滤波的目的------从所有不确定信息中提取有价值的信息。
根据数理统计知识，我们知道这种两个观测值（随机变量）之间的关系可以通过一个协方差矩阵
描述（ 图6 ）。
我们假设系统状态的分布为 高斯分布（正态分布） ，所以在 时刻我们需要两个信息：最佳预估值 及其协方差矩阵 （如式(2)所示）。
下一步，我们需要通过 时刻的状态来预测 时刻的状态。请注意，我们不知道状态的准确值，但是我们的预测函数并不在乎，它仅仅是对 时刻所有可能值的范围进行预测转移，然后得出一个k时刻新值的范围。在这个过程中，位置 和速度 的变化为
我们可以通过一个状态转移矩阵 来描述这个转换关系
同理，我们更新协方差矩阵 为
到目前为止，我们考虑的都是匀速运动的情况，也就是系统没有对WALL-E的运动状态进行控制的情况。那么，如果系统对WALL-E进行控制，例如发出一些指令启动或者制动轮子，对这些额外的信息，我们可以通过一个向量 来描述这些信息，并将其添加到我们的预测方程里作为一个修正。假如我们通过发出的指令得到预期的加速度 ，运动状态方程就更新为
引入矩阵表示为
式中 称为控制矩阵， 称为控制向量（例如加速度 ）。当然，如果没有任何外界动力影响的系统，可以忽略这一部分。
我们增加另一个细节，假如我们的预测转换矩阵不是100%准确呢，会发生什么？如果状态只会根据系统自身特性演变，那样将不会有任何问题。如果所有外界作用力对系统的影响可以被计算得十分准确，那样也不会有任何问题。但是如果有些外力我们无法预测，例如我们在跟踪一个四轴飞行器，它会受到风力影响；或者在跟踪一个轮式机器人，轮子可能会打滑，地面上的突起会使它减速。我们无法跟踪这些因素，而这些不确定事件发生时，预测方程将会失灵。因此，我们将这些不确定性统一建模，在预测方程中增加一个不确定项。
通过这种方式，使得原始状态中的每一个点可以都会预测转换到一个范围，而不是某个确定的点（ 图7.a ）。 可以这样描述------ 中的每个点移动到一个符合方差 的高斯分布里（ 图7.b ）。换言之，我们把这些不确定因素描述为方差为 的高斯噪声，并用 表示。这样就会产生一个新的高斯分布，方差不同，但是均值相同（ 图7.c ）。
通过对 的叠加扩展，得到完整的预测转换方程为
新的预测转换方程只是引入了已知的系统控制因素。新的不确定性可以通过之前的不确定性计算得到。到这里，我们得到了一个模糊的估计范围------通过 和 描述的范围。
我们之前的工作仍然是在使用运动模型一种方法来估计系统的状态，现在，我们要把另一种方法，也就是观测（本问题中为GPS定位）考虑进来，以进一步修正对运动状态的估计（ 图8 ）。
我们用矩阵 来描述观测方法的作用，于是有
再加入观测噪声 ，观测方程为
从控制论的角度出发，我们定义新息（也即观测值与预测值的误差）为
当然我们也知道，观测本身也会存在误差，比如本问题中的GPS定位精度仅有10m. 因此，我们用矩阵 来描述这种不确定性（ 图10 及 图11.a ）。
这时，我们新息的协方差为
现在我们需要把两种方法得到的可能性融合起来（ 图11.b ）。对于任何状态，有两个可能性：1. 传感器的观测值更接近系统真实状态；2. 模型推算的估计值更接近系统真实状态。如果有两个相互独立的获取系统状态的方式，并且我们想知道两者都准确的概率值，于是我们可以通过加权来解决更相信谁的问题（ 图11.c ）。
我们现在知道，系统模型的状态预测 与对系统的状态观测 服从高斯分布，把这个问题抽象一下就是——
根据我们的一个估计准则------ 最小方差估计 ，那么这个问题可以转化为优化问题求解
求导数（差分）得
则 ，从而
当维度高于一维时，我们用矩阵来描述，有
这里的 称为 卡尔曼增益 （Kalman Gain），也就是我们在解决更信任哪种方法时的偏向程度。
如果我们从两个独立的维度估计系统状态，那么根据系统模型的预测为
通过传感器的观测为
我们结合着两种方法得到
由 可知，卡尔曼增益为
将 约去（ 中也含有 项），得
此时的卡尔曼增益实际为
我们最后再来验证一下 估计的无偏性 ——
这里我们设 时刻的真值为 ，由于
由于 （ 从初值而来的无偏传递性 ）可知 ，即卡尔曼滤波满足无偏估计准则。显然，其中要求系统噪声和观测噪声是不相关、零期望的白噪声，且是线性系统，初始时刻的状态估计是无偏的。当这些条件不能满足时，卡尔曼滤波的估计结果是有偏的。
到这里，我们已经获得了卡尔曼滤波的全部要素。我们可以把整个过程总结为3个基本假设
假设一 和 都是零均值高斯白噪声，也即 ，
假设二 与 无关，也即
假设三 系统初值 的均值和方差已知，且 与 均不相关。
以及5个基本方程 方程一 状态预测
方程二 协方差预测
方程三 卡尔曼增益
欢迎分享，转载请注明来源：内存溢出
微信扫一扫
支付宝扫一扫
评论列表（0条）