扩展卡尔曼滤波(EKF)应用于GPS-INS组合导航

格式：docx
大小：13.55 KB
文档页数：3

下载文档原格式

卡尔曼滤波与H∞滤波在INS／GPS组合导航中的应用

０弓Ｉ
舌
式中ｘ（）为状态矩阵
组合导航通常采用传统的卡尔曼（ｌｎ滤Ｋａｍａ）
波方法将各种传感器的信息融合在一起，使得构成组合系统的各项性能指标均优于２个子系统单独工作时的性能。但是在对参数不确定系统和有色噪声情况下，ｌｎ滤波器效果难以令人满意口，Ｋａｍａ］而近年来提出的Ｈ滤波方法对不确定和有色噪声
Ｉ／Ｓ组合导航，何确定ｙ值以更好地提高ＮＳＧＰ如精度是下一步研究的重点。
原理［．安：北工业大学出版社，０７Ｍ］西西２０．
作者简介
参考文献
波算法与Ｈ。滤波算法，过ＶＳ０８编程实现算。通２０
法。对于滤波初值的选取，样频率为１ｏＨｚ下采０，列参数由经验确定：状态Ｘ的初值全部取零，陀螺
２卡尔曼滤波与Ｈ。波方程。滤
将上述Ｉ／Ｓ组合导航模型离散化后分ＮＳＧＰ别建立标准卡尔曼滤波算法与Ｈ滤波算法
具有较强的鲁棒性能，满足人们对性能的要能
ｘ（）一［
８ｖ
８８１ｗｆ］ × ￣
Ｆ￡为连续系统的状态转移矩阵（）
ｏｏ０ｏ
Ｆ＝＝
一
２
求［。研究了Ｉ／Ｓ线性系统的滤波问题，２］ＮＳＧＰ分别用卡尔曼滤波和Ｈ滤波解的实例仿真说明了所提出方法的可行性和正确性。

SINS／GPS组合导航的扩展Kalman滤波算法

２．１组合方式
我们采用ＳＩＮＳ和ＧＰＳ的松耦合方式，利用
航信息，具有不受外界干扰、动态性能好、更新频率
快等优点．但是，陀螺、加速度计的误差导致ＳＩＮＳ的导航参数误差会随着时间的积累而增大，长时间工作后不能保证足够的导航精度ｌ１］．
文献标识码：Ａ
３２６９．２０１３．０４．００１
给出了ＳＩＮＳ和ＧＰＳ组合的仿真．由仿真可见，
１引言
捷联惯性导航系统ＳＩＮＳ（Ｓｔｒａｐ — ｄｏｗｎＩｎｅｒｔｉａｌ
ＳＩＮＳ／ＧＰＳ组合导航系统优于单一的导航系统，完全满足了较高精度、高可靠性的导航要求．
明，该算法简单，容易实现，能满足导航精度要求．
关键词：ＳＩＮＳ／ｌｍａｎ滤波
中图分类号：ＴＮ９６７．２ｄｏｉ：１０．３９６９／ｊ．ｉｓｓｎ．１００６
但由于加速度计的随机常数和白噪声较小，因此通
ＮａｖｉｇａｔｉｏｎＳｙｓｔｅｍ）通过陀螺和加速度计测量得到的载体角速度和加速度数据，经一系列坐标转换、积分和数学计算获得载体的姿态角、位置和速度等导
２ＳＩＮＳ／ＧＰＳ组合导航的系统模型

降阶扩展卡尔曼滤波在INS／GPS导航系统中的应用

ａｈｅｅｃｉｖｄ．ＴｈｅｔｈｗｈｔｔｅＩｏｉｉｎｅｒｒｒｅｕｅｒｍｒｈｎ１０ｍｏ４ａｔｒｅｔｓｓｓｏｔａｈＮＳｐｓｔｒｏｓａｅｒｄｃｄｆｏｍｏｅｔａ０ｔ０ｍｆｅｏｃｒｅｔｎ．Ｔｈｅｄｎｒｏａｅｒｄｃｄｆｏ０．０。ｔ０４ｆｅｏｒｃｉｎ．Ｔｈｃｕａｙｉｏｒｃｉｏｅｈａｉｇｅｒｒｒｅｕｅｒｍ１５ｏ０．３。ａｔｒｃｒｅｔｏｅａｃｒｃｎｒｌａｄｐｔｈｉａｓｎｒａｅｒａｌｙｔｅｉｔｇａｉｎ．ＷｈｌｓｔｏｅｔｈｗｈｔｔｅＴＮＤＳｏｌｎｉｓｌｉｃｅｓｄｇｅｔｂｈｎｅｒｔｃｏｙｏｏｅｅｆｔｓｓｓｏｔａｈ
维普资讯
第２７卷第４期
２００６年７月
兵
工
学
报
Ｖｏ．Ｎｏ．１２７４
பைடு நூலகம்
ＡＣＴＡＡＲＭＡＭＥＮＴＡＲＩＩ
Ｊ１２０ｕ．０６
降阶扩展卡尔曼滤波在ＩＳＧＳ导航系统中的应用Ｎ／Ｐ
ＴＳ（ｒｅｎｒｅｅｍｉａｉｙｔｍ）ｈＤＳｉｔｇａｅ８０ａｄＩｔＣ１４ＮＤｔｕｏｔｄｔｒｎｔｎｓｓｈｏｅ．ＴｅＴＮｎｅｒｔｓＭ￥６ｎＮＳｗｉａＰ／０ｈ
ｅｅｄｄｃｍｐｔｒｓｓｅｐｏｅｓｇｔｅｄｔ．ＴｈＮＳｅｒｒｍｏｅｃｕｉｇｔｅｐｏｅｒｏｔｔｓｍｂｄｅｏｕｅｙｔｍｒｃｓｉｈａａｎｅＩｒｏｄｌｎｌｄｎｈｒｐｒｅｒｒｓａｅｉ

GPS与惯导系统的组合导航技术

谢谢观看
LOGO
GPS/INS
INS:
INS 不仅能够提供载体位置、速度参数,还能提供载体的三维姿态参数,是完全自主的导航方式,在航空、航天、航海和陆地等几乎所有领域中都得到了广泛应用。但是,INS 难以克服的缺点是其导航定位误差随时间累加,难以长时间独立工作。
LOGO
GPS/INS
GPS/INS组合:
LOGO
紧耦合和松耦合
优点:
1.组合结构简单，便于工程实现，便于实现容错 2.两个系统能够独立工作，使得导航系统有一定的余度
缺点:
1. GPS 输出的位置、速度通常是与时间相关的； 2.INS 和 GPS 信息流动是单向的，INS 无法辅GPS。
LOGO
GPS/INS
紧耦合:
紧耦合模式是指利用 GPS 接收机的的原始信息来和惯导系统组合，原始信息一般是指伪距、伪距率、载波相位等。
LOGO
分类:
基于卡尔曼组合数据的融合方法
按照组合中滤波器的设置来分类，可以分成：集中式的卡尔曼滤波分布式的卡尔曼滤波按照对系统校正方法的不同，分为: 开环校正(输出校正) 闭环校正(反馈矫正) 按照组合水平的深度不同，分为：松耦合紧耦合根据卡尔曼滤波器所估计的状态不同，卡尔曼滤波在组合导航中的应用有: 直接法间接法
目录
2 3
LOGO
紧耦合和松耦合
基于卡尔曼滤波的组合方式:
利用卡尔曼滤波器设计 GPS/INS 组合导航系统的方法多种多样按照组合水平的深度不同，分为：松耦合紧耦合
LOGO
紧耦合和松耦合
松耦合:
松耦合模式是指直接利用 GPS 接收机输出的定位信息与 INS 组合，它是一种低水平的组合。位置、速度组合是其典型代表，它采用 GPS 和 INS 输出的位置和速度信息的差值作为量测值。

卡尔曼滤波在GPS中的应用

本科毕业论文 (设计)题目：卡尔曼滤波在GPS定位中的应用学院：自动化工程学院专业：自动化姓名：指导教师：2010年 6月 4日The Application of Kalman Filtering for GPS Positioning摘要本文提出了一种应用卡尔曼滤波的GPS滤波模型。

目前在提高GPS定位精度的自主式方法研究领域，普遍采用卡尔曼滤波算法对GPS定位数据进行处理。

由于定位误差的存在,在GPS动态导航定位中，为提高定位精度，必须对动态定位数据进行滤波处理。

文中在比较分析各种动态模型的基础上，提出了应用卡尔曼滤波的GPS滤波模型，并通过对实测滤波算例仿真，证实了模型的可行性和有效性。

最后提出了卡尔曼滤波在GPS定位滤波应用中的问题和改进思路。

关键词 GPS 卡尔曼滤波定位误差AbstractThis article proposed applies the GPS filter model of the Kalman filtering. At present, to improve GPS positioning accuracy in the autonomous areas of research methods, we commonly use Kalman filter algorithm to process GPS location data.As a result of the position error existence in the GPS dynamic navigation localization, we must carry on filter processing to the dynamic localization data for the enhancement pointing accuracy．In the base of comparing each kind of dynamic model, this article proposed applies the GPS filter model of the Kalman filtering，the actual examples of filter calculation are simulated, it confirmed that the model is feasibility and validity. Finally, this article also proposed the existing problems and improving the idea ofthe applications of Kalman filter in GPS positioning.Keywords GPS Kalman filtering Positioning error目录前言 (1)第1章绪论 (3)1.1GPS的简介及应用 (3)1.2本课题的背景及意义 (5)1.3国内外研究动态及发展趋势 (7)1.4目前GPS定位系统面临着新的困扰和挑战 (5)第2章 GPS全球定位系统及GPS定位误差分析 (8)2.1GPS全球定位系统组成部分 (8)2.1.1 GPS卫星星座 (8)2.1.2 地面支持系统 (9)2.1.3 用户部分 (10)2.2GPS定位原理和测速原理 (16)2.2.1 卫星无源测距定位和伪距测量定位原理 (17)2.2.2 多普勒测量定位原理 (193)2.2.3 GPS测速原理 (214)2.3GPS定位误差分析 (225)2.3.1 星钟误差 (225)2.3.2 星历误差 (225)2.3.3 电离层和对流层的延迟误差 (236)2.3.4 多路径效应引起的误差 (246)2.3.5 接收设备误差 (246)2.3.6 GPS测速误差 (257)第3章卡尔曼滤波理论 (27)3.1卡尔曼滤波理论的工程背景 (27)3.2卡尔曼滤波理论 (28)第4章卡尔曼滤波在GPS定位中的应用 (34)4.1卡尔曼滤波在GPS定位中的应用概述 (34)4.2运动载体的动态模型 (35)4.3卡尔曼滤波模型 (36)4.3.1 状态方程 (36)4.3.2系统的量测方程 (37)4.4滤波仿真和结论 (37)第5章卡尔曼滤波在GPS定位应用中的问题和改进思路 (40)5.1对野值的处理 (40)5.2对状态以及观测噪声方差阵的处理 (41)5.3对观测噪声和测量噪声的处理 (42)结论 (30)谢辞 (31)参考文献 (47)前言自从赫兹证明了麦克斯韦的电磁波辐射理论以后，人们便开始了对无线电导航定位系统研究。

UKF与EKF在GPS_INS超紧组合导航中的应用比较

２超紧组合导航系统非线性模型
状态方程惯性导航系统本质上是非线性的，常用的线性误差模型都是建立在假设姿态误差角是小量的情况下得到的，由于忽略了高阶项，所以当存在较大的导航误差时，线性化误差方程就不能精确描述系统的非线性特性。我们也知道，对于一个非线性系统，滤波的计算量会因为矩阵运算而很大，当状态向量的维数增加，计算量会增加，因此在不损失总体性能的情况下，忽略系统的一些非线性项是合理的。比起速姿态误差对惯性系统的性能影响要严度和位置，
７］卡尔曼滤波以及联邦卡尔曼滤波［。这些方法有模
燉ＧＰＳＩＮＳ组合导航系统因为克服了各自缺点，取长补短，被一致认为是飞行载体较理想的导航系。Ｇ燉ＰＳＩＮＳ组合导航系统按照信息交换或组合程度的不同，又分为松散组合、紧组合和超紧组
段笑菊，薛晓中
南京理工大学，南京（摘２１００９４）
要：针对Ｇ基于四元素法建立了系统的非线性状态方程，利用Ｇ燉ＰＳＩＮＳ超紧组合特点，ＰＳ接收机原始伪距测量信息
对系统状态进行观测，并将Ｅ仿真结果表明ＵＫＦ在姿态、位置估计上精度要优于ＫＦ和ＵＫＦ方法运用到系统进行比较，。ＥＫＦ关键词：超紧组合，非线性，，ＥＫＦＵＫＦ中图分类号：４９３２Ｖ２文献标识码：Ａ
总第３（５－９５３）
·６１ ·

GPS,INS组合导航

GPS/INS 组合导航（仪器科学与工程学院）摘要：GPS/INS 组合导航是用GPS和INS各自的优点进行组合得到的组合导航系统。

它能够拥有GPS的长距离同误差和INS的短距离精确导航的优点，本文是关于GPS/INS组合导航的综述。

关键词：组合导航；惯性导航系统；GPS；INSGPS可以提供全球性的、全天候的、高精度的无源式三维导航定位服务，定位误差不随时间增长，但是GPS的自主性差，需要依靠运营商，受地形建筑的遮蔽信号物的影响，很难做到高精度实时动态控制和导航。

而INS的短期精度高、自主性强、抗干扰能力强，但是长期精度低，导航误差随着时间会逐渐积累。

所以二者的优缺点结合互补，可以实现实时精度高，动态性强，数据更新率高等优点。

1背景1.1 GPS简介GPS是英文Global Positioning System（全球定位系统）的简称。

GPS起始于1958年美国军方的一个项目，1964年投入使用。

20世纪70年代，美国陆海空三军联合研制了新一代卫星定位系统GPS 。

主要目的是为陆海空三大领域提供实时、全天候和全球性的导航服务，并用于情报搜集、核爆监测和应急通讯等一些军事目的，经过20余年的研究实验，耗资300亿美元，到1994年，全球覆盖率高达98%的24颗GPS卫星星座己布设完成。

它有以下的优点[1][4][5]：1、定位精度高，GPS定位精度可以达到0.1～0.0lppm。

定点定位GPS有着这么高的精度可以满足不同情况下，不同需求下的精度需求。

2、范围广，全球定位。

3、适应性强，可在各种恶劣环境中工作，可以24小时工作。

而且无论是高山，深谷，GPS都能够工作。

同样的GPS也有弊端：1、抗干扰能力弱，GPS利用电磁波传递信号，容易受到地形，天气，磁场，电磁波等干扰。

也会受到大气层中对流层和电离层的影响。

2、由于电磁波传播途径被影响，会导致定位时产生误差。

影响精度。

3、自主性差GPS是现在人们生活工作中重要的工具，能够满足人们一定的生活工作需求，但是它明显的缺点也是制约其进一步发展的因素。

基于卡尔曼滤波的GPS与INS位置组合导航及matlab仿真

ˆ (k ) ，则其中，y(k)是能够量测的输出量。问题是根据量测量 y(k)估计出 x(k)。若记 x(k)的估计量为 x (k ) x(k ) x ˆ (k ) x
为状态估计误差，因而（1-4）
(k ) x T (k ) P (k ) Ex
（1-5）
为状态估计误差的协方差阵，显然 P(k)为非负定对称阵。这里估计的准则为根据量测量 y(k), y(k-1), …最优
式中：（1-22）
在以上的计算过程中，将式(1-17)展开得
P (k ) P (k k 1) K (k )CP (k k 1) P (k k 1)C T K T (k ) K (k )CP (k k 1)C T K T (k ) K (k )WK (k )T [ I K (k )C ]P (k k 1) P (k k 1)C T K T (k ) K (k )[CP (k k 1)C T W ]K (k )T
其中：（1-18）
R [ I K (k )C ]P (k k 1)C T K (k )W
（1-19）
如果 K(k)能够使得式(1-17)中的 P(k)取极小值，那么，对于任意的增量ΔK(k)均应有ΔP(k) = 0。要使该点成立，则必须有
R [ I K (k )C ]P (k k 1)C T K (k )W P (k k 1)C T K (k )[CP (k k 1)C T W ] 0
0sintancossintan0cos0sineeieiennenenienmuneienmnmennienuenvvlllrhrhvvllrhrhvvlrhrhvrhvllrhvrh????????????????????????????????????????????????????????????????????????????????????????????????????????2tancossecenueienvllllrh?????????????????????????????????????????????25222速度误差方程及位置误差方程速度误差方程矩阵表达式为

扩展卡尔曼滤波（EKF）应用于GPS-INS组合导航

扩展卡尔曼滤波（EKF）应用于GPS-INS组合导航clear all;%% 惯性-GPS组合导航模型参数初始化we = 360/24/60/60*pi/180; %地球自转角速度，弧度/spsi = 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;0 0 0 -1/Tge 0 0 0 0 0;0 0 0 0 -1/Tgn 0 0 0 0;0 0 0 0 0 -1/Tgz 0 0 0;0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0;0 0 0 0 0 0 0 0 0;]; %状态转移矩阵B=[0 0 0 sigma_ge*sqrt(2/Tge) 0 0 0 0 0;0 0 0 0 sigma_gn*sqrt(2/Tgn) 0 0 0 0;0 0 0 0 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=[1 0 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:lengthX(:,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('滤波后的观测量');。

发射系下SINS-GPS-CNS组合导航系统联邦粒子滤波算法

发射系下SINS/GPS/CNS组合导航系统联邦粒子滤波算法摘要：传统的扩展卡尔曼滤波（Extended Kalman filter,EKF）算法应用于未来高超、空天飞行器的组合导航系统时,因其模型线性化展开会导致模型不准确,从而引起导航精度下降；采用蒙特卡洛方法来实现递推贝叶斯估计问题的粒子滤波（Particle filter,PF）算法能有效避免引入线性化误差,具有一定的优势。

据此,针对高超、空天飞行器在发射过程中通常需要直接获得发射惯性系下的高精度导航参数的需求,提高发射惯性系下弹载组合导航系统滤波算法的精确性就尤为重要,PF滤波算法无需对非线性系统进行线性化展开即可直接实现对非线性系统的状态误差估计。

为此,本文将PF滤波算法引入空天飞行器SINS/GPS/CNS多信息融合组合导航系统,设计了发射系下基于联邦滤波器的PF滤波算法,实现了对组合导航系统状态参数的直接建模估计。

算法仿真结果表明,相较于发射系下SINS/GPS/CNS 组合导航系统联邦EKF滤波算法,PF滤波算法有效提高了组合导航系统滤波精度。

关键词：扩展卡尔曼滤波；粒子滤波；联邦滤波；SINS/GPS/CNS；组合导航中图分类号：V448.2 文献标志码：A 文章编号：1005-2615（2015）03-0319-05SINS/GPS/CNS Integrated Navigation System Federal PF Algorithm in Launch Inertial Coordinate SystemXiong Zhi,Pan Jialiang,Lin Aijun,Du Huajun,Yu FengAbstract：When the traditional extended Kalman filter （EKF）algorithm is used in integrated navigation system of future aircraft, it may lead to the inaccuracy of the model because of linearization and the decreasing of navigation precision. Particle filter （PF） solves Bayes estimation problem by using Monte Carlo method and can effectively avoid bringing in linearization error. Consequently, aimed at the requirement of high accuracy for the navigation system state parameters in launch inertial coordinate system, it is particularly important to improve the accuracy of filter algorithm for missile-borne integrated navigation system. The PF algorithm can directly achieve an error estimation without linearization of nonlinear system. This paper brings PF algorithm into the SINS/GPS/CNS integrated navigation system. PF algorithm is designed based on federal filter and the navigation system state parameters are estimated directly. The algorithm simulation results indicate PF algorithm effectively improves filtering precision compared with EKF algorithm and is very suitable for integrated navigation system.基金项目：国家自然科学基金（xxxx,xxxx,xxxx,xxxx,xxxx）资助项目；江苏省六人人才高峰（2013-JY-013）资助项目；江苏高校优势学科建设工程资助项目；中央高校基本科研业务费专项资金资助项目；南京航空航天大学大学研究生创新基地（实验室）开放基金（kfjjxxxx）资助项目。

1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。

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;
0 0 0 -1/Tge 0 0 0 0 0;
0 0 0 0 -1/Tgn 0 0 0 0;
0 0 0 0 0 -1/Tgz 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;
0 0 0 0 0 0 0 0 0;]; %状态转移矩阵
B=[0 0 0 sigma_ge*sqrt(2/Tge) 0 0 0 0 0;
0 0 0 0 sigma_gn*sqrt(2/Tgn) 0 0 0 0;
0 0 0 0 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=[1 0 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('滤波后的观测量');。