下载文档原格式
卡尔曼滤波算法及其在组合导航中的应用综述摘要:由于描述系统特性的数学模型和噪声的统计模型不准确,不能真实反映物理过程,使模型与获得的观测值不匹配从而会导致滤波器发散。
文章在描述组合导航基本特性和卡尔曼滤波原理的基础上提出了滤波发散的问题并提出了抑制发散的方法,最后介绍了卡尔曼滤波在组合导航中的应用。
关键词:卡尔曼滤波;组合导航;发散随着计算机技术的迅速发展,它有条件提供运算速度高、存贮量大的机载计算机,这为组合导航系统的发展创造了一个很好的技术条件,现代控制理论中最优估计理论的数据处理方法为组合导航系统提供了理论基础。
Kalman滤波是R.E.Kalman于1960年提出的从众多与被提取信号有关的观测量中通过算法估计出所需信号的一种滤波算法。
他把状态空间的概念引入到随机估计理论中,把信号过程视为白噪声作用下的一个线性系统的输出,用状态方程来描述这种输入-输出关系,估计过程中利用系统状态方程、观测方程、系统噪声和观测噪声的统计特性形成滤波算法。
1组合导航系统基本特性描述要描述一个实际系统,首先要对其进行建模,即建立系统的状态方程和测量方程。
对于组合导航系统,要进行滤波计算必须建立数学模型,此模型具有以下特点。
1.1非线性组合导航系统本质上是非线性系统,有时为了减少计算量及提高系统实时性,在某些假设条件下组合导航系统的非线性因素可以忽略,其可以用线性化的数学模型来近似描述。
但当假设条件不满足时,组合导航系统就必须采用能反映自身实际特性的非线性模型来描述。
所以说,非线性是组合导航系统本质的特性。
1.2模型不确定性组合导航系统处于实际运行环境当中时,受系统本身以及外部应用环境不确定性因素的影响,系统实际模型与建立的理论模型不能完全匹配,即组合导航系统具有模型不确定性。
造成系统模型不确定性的主要原因如下:①模型简化。
采用较少的状态变量来描述系统,忽略掉实际系统某些不重要的状态特征。
由此造成模型与实际不匹配。
卡尔曼滤波实验报告2014 年 4 月GPS 静/动态滤波实验一、实验要求1、分别建立GPS 静态及动态卡尔曼滤波模型，编写程序对静态和动态GPS 数据进行Kalman 滤波。
2、对比滤波前后导航轨迹图。
3、画出滤波过程中估计均方差(P 阵对角线元素开根号)的变化趋势。
4、思考：① 简述动态模型与静态模型的区别与联系；② R 阵、Q 阵，P0阵的选取对滤波精度及收敛速度有何影响，取值时应注意什么；③ 本滤波问题是否可以用最小二乘方法解决，如果可以，请阐述最小二乘方法与Kalman 滤波方法的优劣对比。
二、实验原理2.1 GPS 静态滤波选取系统的状态变量为[ ]TL h λ=X ,其中L 为纬度(deg),λ为经度(deg)，h 为高度(m)。
设()w t 为零均值高斯白噪声，则系统的状态方程为：310()w t ⨯=+X(1)所以离散化的状态模型为：,111k k k k k W ---=+X X Φ(2)式中，,1k k -Φ为33⨯单位阵，k W 为系统噪声序列。
测量数据包括：纬度静态量测值、经度静态量测值和高度构成31⨯矩阵Z ，量测方程可以表示为：k k k Z HX V =+(3)式中，H 为33⨯单位阵，k V 为量测噪声序列。
系统的状态模型是十分准确的，所以系统模型噪声方差阵可以取得十分小，取Q 阵零矩阵。
系统测量噪声方差阵R 由测量确定，由于位置量测精度为5m ，采用克拉索夫斯基地球椭球模型，长半径e R 为6378245m ，短半径p R 为6356863m 。
所以R 阵为：2225180()005180()0cos()005p e R R L ππ⨯⎛⎫ ⎪⨯ ⎪ ⎪⨯= ⎪⨯⨯ ⎪ ⎪ ⎪⎝⎭R (4)2.2 GPS 动态滤波动态滤波基于当前统计模型，在地球坐标系下解算。
选取系统的状态变量为Tx x x y y y z z z X x v a y v a z v a εεε⎡⎤=⎣⎦,其中,,,x x x x v a ε依次为地球坐标系下x轴上的位置、速度、加速度和位置误差分量，,y z 轴同理。
数字信号处理实验报告姓名： 专业: 通信与信息系统 学号: 日期：2015.11实验内容任务一：一连续平稳的随机信号()t x ，自相关函数()tx er -=τ，信号()t x 为加性噪声所干扰，噪声是白噪声，测量值的离散值()k z 为已知，s T s 02.0=，-3.2，-0.8，-14，-16，-17，-18，-3.3，-2.4，-18，-0.3，-0.4，-0.8，-19，-2.0，-1.2，-11，-14，-0.9，-0.8，10，0.2，0.5，-0.5，2.4，-0.5，0.5，-13，0.5，10，-12，0.5，-0.6，-15，-0.7，15，0.5，-0.7，-2.0，-19，-17，-11，-14，自编卡尔曼滤波递推程序，估计信号()t x 的波形。
任务二：设计一维纳滤波器。
（1）产生三组观测数据：首先根据()()()n w n as n s +-=1产生信号()n s ，将其加噪（信噪比分别为20dB ，10dB ，6dB ），得到观测数据() n x 1，() n x 2，() n x 3。
（2）估计() n x i ， 1=i ，2，3的AR 模型参数。
假设信号长度为L ，AR 模型阶数为N ，分析实验结果，并讨论改变L ，N 对实验结果的影响。
实验任务一 1. 卡尔曼滤波原理1.1 卡尔曼滤波简介早在20世纪40年代，开始有人用状态变量模型来研究随机过程，到60年代初，由于空间技术的发展，为了解决对非平稳、多输入输出随机序列的估计问题，卡尔曼提出了递推最优估计理论。
它用状态空间法描述系统，由状态方程和量测方程所组成，即知道前一个状态的估计值和最近一个观测数据，采用递推的算法估计当前的状态值。
由于卡尔曼滤波采用递推法，适合于计算机处理，并且可以用来处理多维和非平稳随机信号，现已广泛应用于很多领域，并取得了很好的结果。
卡尔曼滤波一经出现，就受到人们的很大重视，并 在实践中不断丰富和完善，其中一个成功的应用是设计运载体的高精度组合导航系统。
《卡尔曼滤波与组合导航》课程实验报告实验捷联惯导 /GPS 组合导航系统静态导航实验实验序号 3姓名陈星宇系院专业17班级 ZY11172 学 号 ZY1117212日期2012-5-15指导教师宫晓琳成绩一、实验目的① 掌握捷联惯导 /GPS 组合导航系统的构成和基本工作原理； ② 掌握采用卡尔曼滤波方法进行捷联惯导/GPS 组合的基本原理；③ 掌握捷联惯导 /GPS 组合导航系统静态性能；④了解捷联惯导 /GPS 组合导航静态时的系统状态可观测性；二、实验原理（ 1）系统方程 X FX GWXTE NUvEvNvULhx y z x y z其中， E 、 N 、 U 为数学平台失准角；v E 、 v N 、 v U 分别为载体的东向、北向和天向速度误差；L 、 、 h 分别为纬度误差、经度误差和高度误差；x 、 y 、 z、x、y、z 分别为陀螺随机常值漂移和加速度计随机常值零偏。
（下标E 、 N 、 U 分别代表东、北、天）系统的噪声转移矩阵G 为：C b n03 3G03 3C b n9 39 315 6系统噪声矢量由陀螺仪和加速度计的随机误差组成，表达式为：wwwww wTzwxyxyz系统的状态转移矩阵F 组成内容为：F NF SC b n3 3 ，其中 F N 中非零元素为可由惯导误差模型获得。
F S03 3 C b n 。
F069FM03 3 03 39 6（ 2）量测方程量测变量 zV E V NV ULT，，V 、V 、V 、L 、HENU和 H 分别为捷联解算与 GPS 的东向速度、北向速度、天向速度、纬度、经度和高度之差；量测矩阵 H H V H P T03 6 diag R M H , (R N H )cos L,036 ，， H PV 3 3diag 1, 1, 1 0 3 9 ，v v V E v V N v V U v v T H v为量测噪声。
量测噪声方0L H差阵 R 根据GPS的位置、速度噪声水平选取。
卡尔曼滤波实验报告一、实验任务产生含噪声信号X(n)=sin(2*pi*f*n)+w(n),f=0.05,w(n)~N(0,1.2)。
编写程序运用卡尔曼滤波进行去噪处理，要求画出去噪前和去噪后图形，滤波误差及收敛过程。
二、实验程序clc;clear;N=256 ; %信号与噪声的长度离散信号个数w=randn(1.2,N); %产生高斯白噪声，令方差为1.2f=0.05; %实正弦信号频率s=sin(2*pi*f*(0:N-1)) ; %产生正弦信号subplot(311);plot(s);title('有用信号s(n)')grid on;x=s+w;subplot(312);plot(x);title('加噪信号x(n)')grid on;c=[1]; %观测矩阵a=[1]; %状态转移矩阵b=[1]; %输入矩阵H=[1];R=std(w); %R是观测白噪声v(k)的方差Y(1)=20;P(1)=10;for i=1:1:N-1Y(i+1)=a*Y(i)+b*s(i);P(i+1)=a*P(i);Kg(i)=P(i+1)*H'*inv(H*P(i+1)*H'+R);Y(i+1)= Y(i+1)+Kg(i)*(x(i)-H*Y(i+1));P(i+1)=P(i+1)-Kg(i)*H*P(i+1);end;subplot(313);t=1:N;plot(t,Y);title('通过卡尔曼滤波后的估计信号y(n)')grid on;三、实验结果四、实验总结与维纳滤波器实验结果相比，卡尔曼滤波器的输出更加平滑，但是仍没有去除掉曲线中的椒盐噪声点，这一点需要继续改进。
卡尔曼滤波就是根据前一个估计值x^k-1和当前的观测值yk来对信号作递推估计，得到x^k 。
首先建立卡尔曼滤波器的模型，由状态方程和观测方程xk=Akxk-1+wk-1，y k =Ckxk+vk,由此可得到k时刻的预测值x^k’=Ak-1x^k-1与估计值y^k’=Ckx^k’=CkAkx^k-1,定义新息y~k =yk-y^k’，由于wk-1和vk的影响才产生了y~k，为了得到最有估计值，有必要利用一系列矩阵Hk 来校正预测值y^k’，此时x^k= Ak-1x^k-1+Hk(yk- CkAkx^k-1)上式为卡尔曼滤波器的递推方程，这样就可以根据前一个估计值x^k-1和当前观测值yk对信号作递推估计，得到x^k。
卡尔曼滤波报告一、实验任务产生含噪声信号X(n)=sin(2*pi*f*n)+w(n),f=0.05,w(n)~N(0,1.2)。
编写程序运用卡尔曼滤波进行去噪处理，要求画出去噪前和去噪后图形，滤波误差及收敛过程。
二、实验程序%***离散线性时不变系统的状态空间模型为：%***X(k+1)=A*X(k)+B*U(k)+w(k)%***Z(k)=H*X(k)+v(k) 每一个时刻都有噪声加入，算法是实时修正上一时刻的状态值clc; clear;%%产生有用信号s(n),加噪信号x(n)N=256 ; %信号与噪声的长度离散信号个数w=randn(1.2,N); %产生高斯白噪声，令方差为1.2f=0.05; %实正弦信号频率s=sin(2*pi*f*(0:N-1)) ; %产生正弦信号subplot(311);plot(s);title('有用信号s(n)')grid on;x=s+w;subplot(312); plot(x);title('加噪信号x(n)')grid on;c=1; %观测矩阵a=[1]; %状态转移矩阵b=[1]; %输入矩阵H=[1];R=std(w); %R是观测白噪声v(k)的方差%% ***卡尔曼滤波循环Y(1)=20;P(1)=10;for i=1:1:N-1Y(i+1)=a*Y(i)+b*s(i);P(i+1)=a*P(i);Kg(i)=P(i+1)*H'*inv(H*P(i+1)*H'+R);Y(i+1)= Y(i+1)+Kg(i)*(x(i)-H*Y(i+1));P(i+1)=P(i+1)-Kg(i)*H*P(i+1);end;subplot(313);t=1:N;plot(t,Y);title('通过卡尔曼滤波后的估计信号y(n)')grid on;三、实验结果Kalman滤波器的效果是使输出变得更平滑，但没办法去除信号中原有的椒盐噪声，而且，Kalman滤波器也会跟踪这些椒盐噪声点，因此推荐在使用Kalman滤波器前先使用中值滤波去除椒盐噪声。
卡尔曼滤波与组合导航》课程实验报告实验 捷联惯导 /GPS 组合导航系统静态导航实验实验序号 3 姓名 陈星宇系院专业17班级ZY11172学号ZY1117212日期2012-5-15指导教师宫晓琳成绩、实验目的① 掌握捷联惯导 /GPS 组合导航系统的构成和基本工作原理； ②掌握采用卡尔曼滤波方法进行捷联惯导 /GPS 组合的基本原理； ③掌握捷联惯导 /GPS 组合导航系统静态性能；④了解捷联惯导 /GPS 组合导航静态时的系统状态可观测性；、实验原理（ 1）系统方程 X FX GW系统噪声矢量由陀螺仪和加速度计的随机误差组成，表达式为：2）量测方程和 H 分别为捷联解算与 GPS 的东向速度、北向速度、天向速度、纬度、经度和高度之 差；量测矩阵 H H V H P T，H P 03 6 diag R M H, （R N H ）cos L, 036 ，H V 033 diag 1, 1, 1 039，v v V Ev VNv V U v L v v H 为量测噪声。
量测噪声v E v NTvUL hx y z x y z其中， E 、 N 、 U 为数学平台失准角；v E 、 v N 、 v U 分别为载体的东向、北向和天向速度误差；L 、 、 h 分别为纬度误差、经度误差和高度误差；x 、 y 、 z 、 x 、 y 、 z 分别为陀螺随机常值漂移和加速度计随机常值零偏。
（下标 系统的噪声转移矩阵 G 为：E 、N 、 U 分别代表东、北、天）C bn3 3 09 33 3 C n C b9 315 6系统的状态转移矩阵 w w w wF 组成内容为：wzF06N9F SFM，其中 F N 中非零元素为可由惯导误差模型获得。
F SC bn3 3 03 33 3C bn3 396量测变量 z V E V N V U L H ，， V E 、 V N 、 V U 、 L 、XU方差阵R 根据GPS的位置、速度噪声水平选取。
(3)卡尔曼滤波方程状态一步预测：X?k/k 1 k,k 1X?k 1状态估计：X?k X?k/k 1 K k(Z k H k X?k/k 1)滤波增益：K k P k/k 1H k T(H k P k/k 1H k T R k) 1 2 一步预测均方误差：P k/k 1 k,k三、实验内容及步骤1、实验内容① 捷联惯导/GPS 组合导航系统静态导航实验；2、实验步骤2实验准备(由实验教师操作)① 将IMU 安装在大理石实验台上，确认IMU 的安装基准面靠在大理石实验平台上的方位基准尺上；②将IMU 与导航计算机、导航计算机与稳压电源、导航计算机与监控计算机、GPS 接收机与导航计算机、GPS天线与GPS接收机、GPS 接收机与GPS 电池之间的连接线正确连接；③打开GPS 信号转发器；④打开监控计算机中的监控软件；⑤打开稳压电源开关，确认稳压电源输出为28V ；⑥打开捷联惯导/GPS 组合实验系统电源，实验系统开始启动，注意30s 内严禁动IMU；⑦打开GPS接收机电源，确认通过信号转发器可以接收到 4 颗以上卫星；⑧将监控软件设置为“准备”状态，准备时间 5 分钟；⑨准备过程中由实验教师介绍捷联惯导/GPS 组合实验系统的组成、工作原理；1P k 1 k T,k 1 Q k 1 估计均方误差：P k (I K k H k)P k/k 12) 捷联惯导 /GPS 组合导航系统静态导航实验① 实验系统准备 5 分钟之后， 通过监控软件， 将实验系统设置为 “组合导航” 状态；② 记录 IMU 的原始输出，即角增量和比力信息；③ 记录数据过程中，由实验教师介绍采用卡尔曼滤波方法进行捷联惯导 /GPS 组合导 航的基本原理；④ 记录 IMU 数据 5 分钟后，停止记录数据，利用监控计算机中的捷联惯导/GPS 组 合导航软件进行静态导航解算，并显示静态导航结果； 四、实验结果与分析 1、SINS/GPS 组合导航后得纬度曲线和 GPS 导航纬度曲线对比如下图）度（度34.95 34.934.8534.8 34.75 34.7 34.65 34.6 34.55 34.534.45 0 1 2 3 4 5 6 7 84 x 102、SINS/GPS 组合导航后得经度曲线和 GPS 导航经度曲线对比如下图GPS 纬 度 组合导航后纬度 )度 (度 经110.1 110 109.9 109.8 109.7109.6 109.5 109.4 109.3 109.2 109.1GPS 经 度 组合导航后经度4x 103、SINS/GPS 组合导航后得高度曲线和 GPS 导航高度曲线对比如下图35003000 GPS 高 度 组合导航后高度2500 2000（度高1500高 100050084x 104、 SINS/GPS 组合导航后得东向速度曲线和GPS 导航东向速度曲线对比如下图80 60 GPS 东 向 速 度 组 合 导 航 后 东 向 速 度40 20 0 2040）s /m（度速向东-601 2 3 4 5 6 7 8 4x 105、 SINS/GPS 组合导航后得北向速度曲线和GPS 导航北向速度曲线对比如下图604020-20-40-60GPS 北 向 速 度 组合导航后北向速度84x 10 46、 SINS/GPS 组合导航后得天向速度曲线和GPS 导航天向速度曲线对比如下图567GPS 天 向 速 度 组 合 导 航 后 天 向 速 度6 4 2 0 ）s /m（度速向天244x 107、 SINS/GPS 组合导航后航向角曲线、俯仰角曲线和横滚角曲线如下图200 15010050-50-100-150-200组合导航后航向角 组合导航后俯仰角 组合导航后横滚角012 4x 108、纯惯性导航轨迹、 GPS 导航轨迹和 SINS/GPS 组合导航轨迹对比如下图110.1 110109.9109.8 109.7 度 109.6 经109.5 109.4 109.3 109.2109.134.45 34.5 34.55 34.6 34.65 34.7 34.75 34.8 34.85 34.9 34.95 纬度 纯惯性导航轨迹 GPS 导航 轨 迹 组合导航导航轨迹9、平台失准角的估计均方差曲线如下图0.02 度 0.01 00.02 度 0.01 00 1 23北 向 失 准4 角 方 差 5 6784x 10东向失准角方差0 1 2 3 4 5 6 78 0.5天向失准角方差4x 100 1 2 3 4 5 6 7 8 4x 1010、速度和位置的估计均方差曲线如下图东向速度误差方差0.01/s m0.0050 2 4 6 8 4x 10 天向速度误差方差 0.01/s m 0.0050 02468 4 x10 经度误差方差0.1m 0.050 024684 x 10北 向 速 度 误 差 方 差0.01/s m0.0050 2 4 6 84x 10 纬度误差方差0.1m 0.050 2 4 6 84x 10 高度误差方差0.2m 0.10 2 4 6 84x 1011、陀螺漂移的估计均方差曲线和加速度计偏置的估计均方差曲线如下图结果分析：从仿真结果可以看出，滤波之后载体的位置和速度比GPS 输出的位置和速度精度高，载体姿态在滤波校正后结果较好， INS/GPS 组合导航有效地抑制了纯惯性导航的发散，也有效地去除 了 GPS 量测输出的噪声。
东北天方向的速度误差均能够估计出来， 天向陀螺漂移估计不出来， 在动基 座情况下，东向和北向加计零偏、天向平台失准角以及东向和北向陀螺漂移均变得可观测，收敛性变 好。
可见， INS/GPS 是一种较为理想的组合导航方式。
五、源程序clear; clc; % 载入数据卡尔曼 \IMU.dat'); 卡尔曼 \GPS.dat');%%%%%%%%%%定义常数 e=1/298.3; re=6378245;/时小度度0.1 0.1 /时小度度0.1 东向陀螺漂移方差0.05/时小度度x 104天向陀螺漂移方差 北向陀螺漂移方差0.05x 104东向加计偏置方差50gμ0.0550gμgμ4x 104北向加计偏置方差4x 10 44 x 104北向加计偏置方差4x 10 4A (6σ51)s c u ①NI G O1)① >9 G ol )s cu ① ZIΓ1L H ⅛≡≡J M s ⅛≡t ⅛ 世≡1*沢抿&&&&&&&&&&&±f +(0v (ωDUω⅛*c?① *0+L)a)」HQ±f +(0v (ωDUω⅛丄)05」H X M(O H H (S )P O E )七⅛8L *d ¾Ee 6-08L *d '2o -08L *⅛e s①兰H C -¥Pn ≡ωEOEddUO--OFdAe=H C-Duomsodπ>A >x >H U D A l oo α)> <^≡t ⅛鋼曰，翌®旨旨^*<吟擊型>&&&&&&&&&&&&PU ①PU①⅛+Llelue6Helue6φω-φ⅛ILIeEe6Helue6OAGL)q u o七φω-φ=IeIUe6Helue6o ^e o)q u o 4φs φPU①0ddelue6φω-φZAdHelUeCOAGL)q u o七O H H o o )q u o七PU①fai_xFn(5,6)=-vy/Ry;kesai_kf=-pi/2;');ylabel('经度(度) '); figure(3) plot(1:72001,GPS(:,5));hold onplot(t,position_kf(:,3),'r');hold on legend('GPS高度','组合导航后高度');ylabel('高度( m)'); figure(4) plot(1:72001,GPS(:,6));hold onplot(t,velocity_kf(:,1),'r');hold on legend('GPS东向速度','组合导航后东向速度');ylabel('东向速度( m/s)'); figure(5)plot(1:72001,GPS(:,7));hold on plot(t,velocity_kf(:,2),'r');hold on legend('GPS北向速度','组合导航后北向速度');ylabel('北向速度( m/s)'); figure(6) plot(1:72001,GPS(:,8));hold onplot(t,velocity_kf(:,3),'r');hold on legend('GPS天向速度','组合导航后天向速度');ylabel('天向速度( m/s)') figure(7)plot(t,attitude_kf(:,1));hold on plot(t,attitude_kf(:,2),'r');hold onplot(t,attitude_kf(:,3),'g');hold on legend('组合导航后航向角','组合导航后俯仰角','组合导航后横滚角');ylabel(' 度'); figure(8)plot(position(:,1),position(:,2));hold on plot(GPS(:,3),GPS(:,4),'r');hold on plot(position_kf(:,1),position_kf(:,2),'g');hold on legend('纯惯性导航轨迹','GPS导航轨迹','组合导航导航轨迹');xlabel('纬度'); ylabel('经度');figure(9) subplot(3,1,1);plot(t,p_kf(:,1));title(' 东向失准角方差');ylabel(' 度');subplot(3,1,2);plot(t,p_kf(:,2));title(' 北向失准角方差');ylabel(' 度');subplot(3,1,3);plot(t,p_kf(:,3));title(' 天向失准角方差');ylabel(' 度'); figure(10) subplot(3,2,1); plot(t,p_kf(:,4));title(' 东向速度误差方差');ylabel('m/s');subplot(3,2,2); plot(t,p_kf(:,5));title(' 北向速度误差方差');ylabel('m/s');subplot(3,2,3); plot(t,p_kf(:,6));title(' 天向速度误差方差');ylabel('m/s');subplot(3,2,4); plot(t,p_kf(:,7));title(' 纬度误差方差');ylabel('m'); subplot(3,2,5); plot(t,p_kf(:,8));title(' 经度误差方差');ylabel('m');subplot(3,2,1);plot(t,p_kf(:,10));title(' 东向陀螺漂移方差');ylabel(' 度/小时'); subplot(3,2,2);plot(t,p_kf(:,11));title(' 北向陀螺漂移方差');ylabel('度/小时'); subplot(3,2,3);plot(t,p_kf(:,12));title(' 天向陀螺漂移方差');ylabel(' 度/小时'); subplot(3,2,4);plot(t,p_kf(:,13));title(' 东向加计偏置方差');ylabel(' μg'); subplot(3,2,5);plot(t,p_kf(:,14));title(' 北向加计偏置方差');ylabel(' μg'); subplot(3,2,6);plot(t,p_kf(:,15));title(' 北向加计偏置方差');ylabel(' μg');。
