基于自适应Kalman滤波算法的联邦滤波器(1)

Kalman滤波原理及算法kalman滤波器一(什么是卡尔曼滤波器卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯, 我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》(线性滤波与预测问题的新方法)。

二.卡尔曼滤波器算法的介绍以下是卡尔曼滤波器核心的5个式子。

X(k|k-1)=A X(k-1|k-1)+B U(k) (1)P(k|k-1)=A P(k-1|k-1) A’+Q (2)X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) (3)Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) (4)P(k|k)=(I-Kg(k) H)P(k|k-1) (5)下面我们详细介绍卡尔曼滤波的过程。

首先，我们要引入一个离散控制过程的系统。

该系统可用一个线性随机微分方程来描述:X(k)=A X(k-1)+B U(k)+W(k)再加上系统的测量值:Z(k)=H X(k)+V(k)上两式子中，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量。

A和B是系统参数，对于多模型系统，他们为矩阵。

Z(k)是k时刻的测量值，H是测量系统的参数，对于多测量系统，H为矩阵。

W(k)和V(k)分别表示过程和测量的噪声。

他们被假设成高斯白噪声(White Gaussian Noise)，他们的covariance分别是Q，R(这里我们假设他们不随系统状态变化而变化)。

对于满足上面的条件(线性随机微分系统，过程和测量都是高斯白噪声)，卡尔曼滤波器是最优的信息处理器。

下面我们来用他们结合他们的covariances来估算系统的最优化输出。

首先我们要利用系统的过程模型，来预测下一状态的系统。

第8章联邦滤波和自适应滤波第8章联邦滤波和自适应滤波在组合导航中的应用8.1联邦卡尔曼滤波组合导航系统可提高系统的任务可靠性和容错性能。

因为组合导航中有余度的导航信息，如组合适当，则可利用余度信息检测出某导航子系统的故障，将此失效的子系统隔离掉，并将剩下的正常的子系统重新组合（系统重构），就可继续完成导航任务。

组合导航系统还可协助惯导系统进行空中对正和校准，从而提高飞机或其他载体的快速反应能力。

联邦卡尔曼滤波理论是美国学者 Carlson于1998年提出的一种特殊形式的分布式卡尔曼滤波方法。

它由若干个子滤波器和一个主滤波器组成，是一个具有分块估计、两步级联的分散化滤波方法，关键在于它采用信息分配原理。

它需要向各子滤波器分配动态信息，这些信息包括两大类：状态方程的信息和观测方程的信息。

8.1.1联邦卡尔曼滤波器结构运动方程的信息量与状态方程中过程噪声的协方差阵成反比，过程噪声越弱，状态方程就越精确。

因此，状态方程的信息量可以用过程噪声协方差阵的逆 Q1来表示。

此外，状态估计的信息量可用状态估计协方差阵的逆P1表示，测量方程的信息量可用测量噪声协方差阵的逆 R1表示。

如果把局部滤波器i的状态估计矢量、系统协方差阵、状态矢量协方差阵分别记为召、Q i、R,i 1,2, ,n，主滤波器的状态估计矢量、系统协方差阵、状态矢量协方差阵分别记为）?m、Q m、P m,假定按以下规则将整体信息分配至各局部滤波器，即P1）? p1 1）?1 p21）?2叮冷PrJX m 斤刃（8.1）Q 1Q11Q21Q n1Q m1Q i1i Q 1（ 8.2）P 1P11P21P n1P m1P i 1i P 1（ 8.3）其中，i是信息分配系数，必须满足下列条件:在设计联邦卡尔曼滤波器时，信息分配系数的确定至关重要，不同的值会有不同的滤波器结构和特性（容错性、最优性、计算量等）。

令i 1/ i （i 1,2……N,m ）,则它的的几种主要结构可简要地表达如下：（1）第一类结构（m i =1/ （ N + D ，有重置），如图8.1所示图8.1 联邦滤波器第一类结构这类结构的特点是：信息在主滤波器和各子滤波器之间平均分配。

卡尔曼滤波器介绍摘要在1960年，R.E.Kalman发表了关于递归解决线性离散数据滤波器的著名论文，从那时间起，由于在数字计算的大部分提高，Kalman滤波器已成为广泛研究和应用的学科，尤其是自动或辅助导航系统。

Kalman滤波器是一套数学等式，它提供了一种有效的以最小均方误差来估计系统状态的计算(递归的)方法。

它在以下几方面是非常强大的：它支持过去、现在、甚至将来估计，甚至在系统准确模型也未知的情况下。

本文的目的是提供一种对离散的Kalman滤波器的实用介绍。

这些介绍包括对基本离散kalman滤波器、起源和与之相关的简单(有形)的带有真实数字和结果的描述和讨论。

1、离散的kalman滤波器在1960年，R.E.Kalman发表了关于递归解决线性离散数据滤波器的著名论文，从那时间起，由于在数字计算的大部分提高，Kalman滤波器已成为广泛研究和应用的学科，尤其是自动或辅助导航系统。

关于kalman滤波器一般方法的友好介绍可以在〔maybeck79〕的Chapter.1中找到，但是更完整部分的讨论能在〔Sorenson70〕中发现，它还包括许多有趣的历史解释。

在〔Gelb74；Grewal93；Maybeck79；Lewis86；Brown92；jacobs93〕中有更多参考。

估值过程Kalman滤波器解决估计离散时间控制过程的状态X∈R n的一般性问题，定义线性随机差分方程其中，测量值Z∈R m，定义为随机变量W K和V K各自表示系统噪声和测量噪声，我们假定它们为相互独立的、白噪声且为正常概率分布在实际中，系统噪声协方差矩阵Q和测量噪声协方差矩阵R可能随过程和测量时间而改变，无论怎样，我们在这里假定它们是常量。

在差分方程（1.1）中，n×n阶矩阵A与前一时刻（K－1）和当前时刻K相关，这里缺少传递函数或系统噪声。

注意的是，在实际中，A可能随各自时刻改变，但这里我们假定其为常量，n×l阶矩阵R与非强制性输入U∈R l和状态x有关，在测量公式（1.2）中，m×n阶矩阵H 与状态及测量值Z K有关，在实际中，H可能随各自过程或测量时刻而改变，这里假定它们是常数。

一章一、导航与制导的区别：导航：测量载体的导航参数（俯仰角、滚转角、偏航角），位置、速度和加速度。

制导：根据预先规划好的航线，自动引导载体到达目的地的技术和方法。

二、导航技术：1.惯性导航：加速度计：加速度到速度到位置。

（有误差积累，故组合）角度测量陀螺：测量角度（定轴性和进动性）惯性执行机构：力矩陀螺、惯性动量轮。

优点：自主性强、短时间内精度高、连续提供位置与速度与加速度和姿态信息缺点：误差随时间积累、价格昂贵。

2.天文导航：通过观测天体和不同的时刻的同一天体。

优点：完全自主，误差不积累、不仅可以得到位置信息还可以得到姿态信息。

缺点：定位信息不够高，输出信息不连续。

3.卫星导航（无线电导航）优点：精度高、误差无积累、全球全天候全天时。

缺点：输出不连续、不输出姿态信息。

三、惯导系统误差：1.确定性误差：惯性器件常值误差、安装误差标度因数误差与加速度与关的误差2.随机误差：随机常数一阶马尔可夫过程、白噪声加速度计与陀螺漂移Kalmen滤波就是对付随机误差。

Kalmen滤波的条件:线性系统非高斯噪声（白噪声）四、Kalmen滤波对线性系统与非高斯噪声系统的处理1.非线性高斯噪声：EKF UKF2.非高斯线性：PF3.非线性与非高斯：UPF二章、SIN/GPS的组合导航IMU：高精度惯性测量单元一、捷联惯导系统：1.IMU的误差标定与补偿（被积函数）标定原理：a、建立陀螺和加速度计误差方程（误差方程如何建立的？六个方程）b、转台试验求解误差方程中的系数2.SINS的静基座初始对准（积分初值）？：积分运算必须知道初始值粗对准：时间是标准。

精对准：精度是指标。

3. 惯导系统的误差根源是：加速度计偏置与陀螺漂移，属随机误差（？标定误差，初始对准？）消除随机误差用卡拉曼滤波。

4. 惯导系统误差方程1. 计算地理坐标系法 Ψ角误差方程（？？？？？？？？ 方程的建立及化简推导）δV 、r 和Ψ分别为速度、位置和姿态矢量 Ω为地球自转角速度ω为导航坐标系相对惯性坐标系的角速度矢量 ▽是加速度计常值偏值，ε是陀螺常值漂移 f 是比力，△g 是重力矢量计算误差，2. 真实地理坐标系法Ψ角误差方程：在静基座条件下，最终可得：静基座条件下的误差方程：二、kalman 方程的建立（作业）1.建立系统方程Vr r δρ=⨯+.()gf V V ∆+⨯Φ-∇=⨯+Ω+δωδ.εωψ=ψ⨯+.LE E δ+ψ=ΦL N N cos δλ+ψ=ΦLD D sin δλ-ψ=ΦL L L NN ⋅-+ψ=Φsin cos δλλδL EE δ+ψ=ΦL L L DD ⋅--ψ=Φcos sin δλλδN N DE E N R V L R V L ε+ψ+⎪⎭⎫ ⎝⎛+Ωψ-=ψtan sin E E D E N E R V L L R V L ε+⎪⎭⎫ ⎝⎛+Ωψ+⎪⎭⎫ ⎝⎛+Ωψ=ψcos tan sin D E E N N D R V L R V ε+⎪⎭⎫ ⎝⎛+Ωψ-ψ-=ψcos N E E N L L L R V εδ+ΩΦ-Ω-=Φsin sin E D N N E L L R V ε+ΩΦ+ΩΦ+-=Φcos sin D E E D L L L L RV εδ+ΩΦ-Ω--=Φcos cos tan N V RL δδ1= L RV E sec δλδ=2.建立量测方程（那个可以观测？）3.离散kalman 滤波方程 三、静基坐对准提高静基座对准的精度和速度的方法： 四、GPS 动态滤波的数学方程什么意思？ 五、最小二乘法最优估计（只要使函数值最小即可） 最小方差估计（方差最小） 最小二乘估计：三章kalman 滤波原理（课本二五章）Kalman 方程就是递推线性最小方差估计。

9、Kalman 滤波直观推导Kalman 滤波实质是线性最小方差估计]X ~X ~[E min T k k随机线性离散系统方程为1k 1k ,k 1k 1k ,k k W X X ----Γ+Φ=k k k k V X H Z +=过程噪声和观测噪声的统计特性，假设满足如下条件⎪⎪⎩⎪⎪⎨⎧=δ==δ==0]V W [E R ]V V [E ,0]V [E Q ]W W [E ,0]W [E T j k kj k Tj k k kj k T j k k 设k 时刻得到k 次观测值k 1Z Z ，且找到了1k X -的一个最优线性估计1k Xˆ-，即1k Xˆ-是1k 1Z Z - 的线性函数。

(1)k 时刻系统状态的预测估计值1k 1k ,k 1k ,k X ˆXˆ---Φ= (2)k 时刻系统观测值的预测估计值1k ,k k 1k ,k X ˆH Zˆ--= (3)获得k 时刻观测值k Z ，观测值与预测估计1k ,k Zˆ-之间误差 k1k ,k k 1k ,k k k k k 1k ,k k k 1K ,k k k V X ~H X ˆH V X H X ˆH Z ZˆZ Z ~+=-+=-=-=----造成误差的原因为预测估计1K ,k Zˆ-和观测值k Z 可能都存在误差。

(4)修正k 时刻状态的预测估计值，k K 为滤波增益矩阵)X ˆH Z (K X ˆXˆ1k ,k k k k 1k ,k k ---+= (5)获取k 时刻观测值k Z 前后对k X 的估计误差分别为1k ,k k 1k ,k XˆX X ~---= k k k XˆX X ~-= (6)根据最优滤波方差阵k P 求最优滤波增益矩阵k K 。

]X ~X ~[E P T k k k =k k 1k ,k k k k k 1k ,k k k 1k ,k 1k ,k k k k k k 1k ,k 1k ,k k k k 1k ,k k k V K X~]H K I [V K X ~H K X ~]X ˆH V X H [K X ~]X ˆH Z [K XˆX X~--=--=-+-=---=-------注：0]X ~V [E ,0]V X ~[E T 1k ,k k Tk 1K ,k ==--T kk k T k k 1k ,k k k T kTk k k T k k T 1k ,k 1k ,k k k T k T k k k T k k T 1k ,k k k T kTk 1k ,k k k T k k T 1k ,k 1k ,k k k T k k k TkT k k k T k k T 1k ,k k k T k T k 1k ,k k k T k k T 1k ,k 1k ,k k k T k k 1k ,k k k k k 1k ,k k k T k k K R K ]H K I [P ]H K I [K ]V V [E K ]H K I ][X ~X ~[E ]H K I [K ]V V [E K ]H K I ][X ~V [E K K ]V X ~[E ]H K I []H K I ][X ~X ~[E ]H K I []X ~X ~[E P K V V K ]H K I [X ~V K K V X ~]H K I []H K I [X ~X ~]H K I [}V K X ~]H K I }{[V K X ~]H K I {[X ~X ~+--=+--=+------==+------=----=-------------求最优增益k K 。

万方数据万方数据９１０压电与声光表１两种自适应滤波算法增加的计算量法则言裟象本文的自适应滤波算法乘法／次∥＋３ｍ２九＋４ｍ２＋ｎ２ｍ除法／次ｌ加法／次ｍ３＋３ｍ２，ｚ＋竹２仇一ｍ行一ｍ减法／次２ｍ２＋３指数函数／次０ｍ２＋ｍ２ｍ２＋２ｍ１１从表ｌ可见，在每一滤波周期内，本文提出的自适应Ｋａｌｍａｎ滤波比常规Ｋａｌｍａｎ滤波增加了ｍ２＋优次乘法运算、２次除法运算、ｍ２＋２ｍ次加法运算、１次减法运算和１次指数函数运算。

而该算法和简化Ｓａｇｅ—Ｈｕｓａ自适应滤波相比，减少了ｍ３＋３ｍ２卵＋３ｍ２＋７２２ｍ—ｍ次乘法运算、ｍ３＋３ｍ２咒＋豫２ｍ一优２一ｍｎ一３ｍ次加法运算和２ｍ２＋２次减法运算；增加了１次除法和１次指数函数的运算。

因此，文本的自适应Ｋａｌｍａｎ滤波算法再增加了一定的计算量后即可实现滤波的自适应，比简化Ｓａｇｅ－Ｈｕｓａ自适应滤波的计算量小，特别是系统维数越高，节省的计算量也越多。

在组合系统误差模型和噪声统计特性未知时，利用本文提出的算法进行组合导航，既可保证滤波的自适应性，又有利于提高系统的实时性。

２自适应Ｋａｌｍａｎ滤波算法的应用通过在惯导／双星组合导航系统中应用，对本文提出的自适应Ｋａｌｍａｎ滤波算法与常规Ｋａｌｍａｎ滤波和Ｓａｇｅ－Ｈｕｓａ自适应滤波作了进一步比较。

２．１惯导／双星组合系统数学模型取惯导系统的３个平台误差角、２个速度（东向、北向）误差、２个位置（东向、北向）误差和３个陀螺漂移误差为状态构成１０维的状态方程¨１文（￡）一Ｆ（￡）Ｘ（￡）＋Ｗ（￡）（２１）式中Ｆ一［卵，＂，蜘，８ＶＥ，６ｙＮ，占Ｌ，淑，ｅ疆，￡ｗ，￡叫］为状态ｘ的转置，Ｅ、Ｎ和Ｕ为东北天坐标系，他、＂和９ｕ为惯导系统的平台误差角；艿Ｖ￡和６ＶＮ为ＩＮＳ的速度误差；艿Ｌ和淑为惯导系统的经度与纬度误差；￡幄、￡州和ＥｂＵ为３个轴向陀螺的漂移误差；Ｗ（￡）为系统噪声。

由于双星接收机仅输出位置（经度和纬度），所以用惯导系统解算的位置与双星输出的位置之差作为量测量，可得量测方程［６］厂（ＬＩ—ＬＤ）ＲＭ］～Ｌ（Ａｌ—ＡＤ）ＲＮｃｏｓ（Ｌ）Ｊ厂ＲＭ占Ｌ＋ＮＮ］【－ＲＮｃｏｓ（Ｌ）叛＋ＮＥｊ２Ｈｘｏ）＋ｙｏ’（２２’式中Ｈ一［０２×５ｄｉａｇ［－ＲＮＲＮｃｏｓ（Ｌ）］０２ｘ３］；Ｖ７＝［ＮＮＮｅ］；Ｌ－、Ａ一为惯导系统输出的纬度、经度；Ｌ。

信息融合技术在水下组合导航系统中的应用袁小龙【摘要】要实现水下精确导航，采用单一导航方法，其精度、可靠性无法满足需要。

将多种导航技术适当融合，可以取长补短提高导航精度。

以捷联惯性导航系统作为组合导航系统的主导航设备， GPS导航、多普勒导航等为辅助导航设备，利用联邦Ka lman滤波技术对水下组合导航系统进行信息融合，充分融合多导航传感器信息，提高远程自主水下航行器的导航精度。

%It is difficult to realize precise underwater navigation by using single navigation method .The integration of a variety of navigation technologies can improve the preciseness of navigation .This paper presents a combination of navigation aids which takes SINS as the main navigation equipment , GPS and DVL as assistant navigation . Fusing the information from multiple sensors by Federal Kalman filter technology can provide precise navigation for underwater vehicle.【期刊名称】《广州航海高等专科学校学报》【年(卷),期】2013(000)001【总页数】4页(P18-20,24)【关键词】水下航行器;组合导航;捷联惯性导航;GPS导航;多普勒导航;联邦滤波器;信息融合【作者】袁小龙【作者单位】海军兵种指挥学院研究生队，广东广州510430【正文语种】中文导航定位技术是水下航行器安全航行的重要保障.在组合导航系统中采用了多种导航系统，因此会产生多个不同导航定位信息.如何利用这些不同的定位信息实现航行器的精确导航，数据融合为这些信息进行融合决策处理提供了解决方法.基于Kalman滤波算法的联邦卡尔曼滤波器可以将来自多个传感器的信息进行最优组合，为水下航行器提供精确导航定位.本文采用捷联惯性导航系统、GPS导航系统、多普勒测速仪、磁航仪实现水下组合导航系统.组合导航系统的基本构成如图1所示. 图1 组合导航系统的基本构成1 滤波器的设计多源信息融合是将来自多种信息源的信息统一合并的过程，在多传感器感知系统中，各种信息源提供的信息都有一定程度的不确定性，因此必须找到一种能按一定判据原则将信息有效综合处理的方法，卡尔曼滤波是一种线性无偏的、以误差方差最小为标准的最优的估计算法，目前广泛应用于多源信息融合技术中.应用于卡尔曼滤波的多传感器信息融合系统有两种主要的滤波结构:集中式卡尔曼滤波和联邦式卡尔曼滤波，联邦滤波器是在分散化滤波基础上提出的，分散化滤波由于设计的灵活性、计算量小、容错性好而受到重视.相对于分散化滤波，联邦滤波器的算法复杂性进一步降低，容错性和可靠性进一步加强.所以联邦滤波器广泛应用于水下组合导航中［1］.水下组合导航系统的联邦滤波器由SINS、GPS、DVL以及磁航向仪为系统的传感器，其中SINS作为系统的主导航，其余作为辅助导航，以SINS作为公共参考系统，SINS分别与三个辅助导航组成三个子滤波器.分别为:SINS和GPS组成的位置姿态子滤波器，SINS和DVL组成的速度子滤波器，SINS和磁航向仪组成的航向子滤波器.这三个滤波器都采用卡尔曼滤波器结构进行信息融合处理，三个滤波器在输入一个主滤波器进行信息融合.联邦卡尔曼滤波器结构图如图2所示.［2］图2 联邦卡尔曼滤波器结构图2 水下组合导航系统模型的建立2.1 SINS系统误差模型的建立捷联惯性导航系统根据牛顿惯性原理，利用陀螺仪和加速度计测量出载体的角速度和加速度，经过积分等运算获得运载体的位置、速度及姿态信息.捷联惯性导航系统原理图如图3所示.图3 捷联惯性导航系统原理图捷联惯性导航作为主要导航设备，根据系统的工作特点，选择位置误差、速度误差、姿态误差、陀螺漂移和加速度零偏作为状态量:式中δVE、δVN、δVU分别是东向、北向、天向速度误差;φE、φN、φU分别是东向、北向、和天向失准角; δL、δλ、δh分别是纬度、经度和水深度误差;εgx、εgy、εgz分别是x，y向的加速度计偏置;▽ax、▽ay、▽az分别是x，y，z向的陀螺漂移.选取东北天坐标系作为导航坐标系，载体坐标系x轴沿航行器横轴指向右舷，y轴沿航行器纵轴指向前，z轴垂直x与y轴所确定的平面构成右手坐标系.系统的状态方程:在SINS系统中，由于加速度计、陀螺仪均安装在机体上，所以加速度计和陀螺感受到的机体坐标中的误差需转换为导航坐标中的坐标.系统的噪声矢量为:其中FN为对应9个基本导航参数的系统矩阵，FS、FM分别为:2.2 子滤波器状态方程和测量方程1)SINS/DVL子滤波器误差模型［3］.状态方程:量测方程:其中:H1=其中HD=－Cnb，VDN，VDU，VDE为DVL测速仪所测得的速度.2)SINS/GPS子滤波器误差模型.状态方程:在位置和速度组合模式中，其测量值有2组.1组为位置测量值，即SINS给出的经纬度、高度信息和GPS接收机给出的相应的信息的差值为1组测量值，而2个系统给出的速度差值为另1组测量值.表示SINS的位置信息为［4］:GPS的位置信息:其中:Lt，λt为真实位置信息，NN，NE为GPS位置观测误差.SINS速度信息:GPS接收机给出的信息:其中ME，MN为GPS速度观测误差.由定义位置、速度测量矢量，确定量测方程:3)SINS/磁航仪航向子滤波器状态方程:量测方程:MCP航向输出以及捷联惯导航向信息分别为:量测矢量:其中VIM为量测白噪声.3 信息融合过程信息融合就是将各个子滤波器的估计信息按一定信息融合规则进行分析与综合，以得到全局估计［5］.联邦卡尔曼滤波的计算过程如下［5］:1)给定全局状态的初始值X0初始协方差阵P0，系统噪声协方差阵Q0，进行信息分配，分配系数满足其中m=1，2，…，N，n.2)子滤波器各自进行局部滤波［6］.观测更新:滤波更新:3)主滤波器对子滤波器信息进行融合.在进行信息融合过程中，如果P值越大说明其所在的子滤波器的滤波估计精度越低，在融合全局估计的权值就越小，对于系统估计值的影响越小，反之相反.参考文献:［1］张国良，曾静.组合导航原理与技术［M］.西安:西安交通大学出版社，2008.［2］周坤芳.现代舰船导航技术［J］.海军广州舰艇学院学报，2003.6.［3］杨晓东，王炜.地磁导航原理［M］.北京:国防工业出版社，2009.［4］朱海，莫军.水下导航信息融合技术［M］.北京:国防工业出版社，2002. ［5］范欣，张福斌，张永清.多传感器信息融合的水下航行器组合导航方法［J］.火力与指挥控制，2011(3):78.［6］李佩娟，徐晓芳，张小飞.智能Kalman滤波在水下地形组合导航系统中的应用［J］.中国惯性技术学报，2011(10):79.。