传统组合导航中的实用Kalman滤波技术评述

严恭敏，邓 瑀

(西北工业大学自动化学院，西安 710072)

0 引言

估计理论是概率论与数理统计的一个分支，它是根据受扰动的观测数据来提取系统某些参数或状态的一种数学方法。1795年，高斯提出了最小二乘法；1912年，费歇尔(R.A.Fisher)提出了极大似然估计法，从概率密度的角度考虑估计问题；1940年，维纳提出了在频域中设计统计最优滤波器的方法，称为维纳滤波，但它只能处理平稳随机过程问题且滤波器设计复杂，应用受到很大限制；1960年，卡尔曼基于状态方程描述提出了一种最优递推滤波方法，称为Kalman滤波，它既适用于平稳随机过程，也适用于非平稳过程，一经提出便得到了广泛应用。在Kalman滤波器出现以后，针对随机动态系统的估计理论的发展基本上都是以它的框架为基础的一些扩展和改进[1]。

Kalman滤波器最早和最成功的应用实例便是在组合导航领域。惯性导航系统(Inertial Navigation System，INS)是最重要的一种导航方式，它能提供姿态、方位、速度和位置，甚至还包括加速度和角速率等导航信息，可用于运载体的正确操纵和控制。惯导具有自主性强、动态性能好、导航信息全面且输出频率高等优点，但其误差随时间不断累积，长期精度不高。相比而言，全球导航卫星系统(Global Navigation Satellite System，GNSS)的优点是精度高且误差不随时间增大，缺点是导航信息不够全面、信号容易受到干扰、在室内等环境下接收不到卫星信号而无法使用。惯导和卫导之间具有很强的互补性，惯导/卫导组合导航提高了系统整体的精度和可靠性，被公认为是最佳的组合导航方案[2]。除了惯导和卫导外，传统的辅助导航方法有里程仪(或多普勒计程仪)、高度表和地磁方位(多用于低精度场合)，当然还有天文导航、地图/重力/地磁匹配导航等，但是后者不太常用或应用领域相对比较狭窄。随着机器人和自动驾驶技术的兴起，激光雷达、视觉图像、即时定位与地图构建(Simultaneous Localization and Mapping，SLAM)、因子图(factor graph)和人工智能(Artificial Intelligence，AI)等导航手段或数据处理方法不断涌现，组合导航和信息融合新技术的研究和发展方兴未艾[3-6]。限于笔者的知识面，论文主要讨论了传统Kalman滤波组合导航算法，以惯导/卫导组合为例，指出实际Kalman滤波应用中可能遇到的问题及其解决思路，希望能为工程技术人员提供一些有实用价值的参考。

1 Kalman滤波基本原理

给定Kalman滤波随机系统的状态空间模型

(1)

式中，Xk是n维的状态向量；Zk是m维的量测向量；Φk/k-1、Γk-1和Hk是已知的系统结构参数，分别称为n阶的状态一步转移矩阵、n×l阶的系统噪声分配矩阵、m×n阶的量测矩阵；Wk-1是l维的系统噪声(或称过程噪声)向量，Vk是m维的量测噪声向量，两者都是零均值的高斯白噪声向量序列，且它们之间互不相关，即满足

(2)

式中，δkj为克罗内克函数。式(2)是Kalman滤波状态空间模型中对于噪声要求的基本假设，一般要求Qk是非负定的且Rk是正定的，即有Qk≥0且Rk>0。实际上，通过选择合适的噪声分配阵Γk-1，可以保证Qk总是正定的[1]。

经过推导，标准(或称传统)Kalman滤波包含5个基本公式，分别为：

1)状态一步预测

(3)

2)状态一步预测均方误差阵

(4)

3)滤波增益

(5)

4)状态估计

(6)

5)状态估计均方误差阵

Pk=(I-KkHk)Pk/k-1

(7)

其中，式(3)和式(4)统称为时间更新，式(5)～式(7)统称为量测更新；式(3)和式(6)组成滤波计算回路(一阶矩计算)，式(4)、式(5)和式(7)组成增益计算回路(二阶矩计算)。注意到滤波计算回路受增益计算回路的影响，而滤波计算回路不对增益计算回路产生任何影响。

对于可观测性较强的状态分量，对应的状态初值和均方误差阵设置偏差容许适当大些，它们随着滤波更新将会快速收敛，如果均方误差阵设置过小，则当初始状态误差较大时会使状态收敛速度缓慢，变为有偏估计。而对于可观测性较弱的状态，对应的状态初值和均方误差阵应该设置得尽量准确，如果均方误差阵设置过大，容易引起状态估计过程中的有偏或剧烈波动；反之，如果均方误差阵设置过小，同样会使状态收敛速度变慢，这两种情况下均方误差阵计算值都不宜用于评估相应状态估计的精度。对于不可观测的状态分量，其状态估计及其均方误差阵不会随滤波更新而变化，即不会有滤波效果。例如在不转位的惯导Kalman滤波初始对准中，等效水平加速度计随机常值零偏(等效东向陀螺随机常值漂移)的方差设置太大会影响水平(方位)失准角的估计，因为两者是相互依赖的且通常认为前者的可观测性较弱，只有前者设置得适当的小，后者的估计精度才会比较高。

2 惯导/卫导组合导航系统建模

常用的Kalman滤波是离散形式的，但在实际系统分析和建模时，多是以连续时间形式表示。惯导/卫导组合导航Kalman滤波的连续时间随机系统模型如下

(8)

(9)

其中

式(8)是一种比较常用的惯导/卫导组合15维状态建模，主要考虑了惯性器件典型误差和惯导导航参数解算误差，且将卫导测量误差视为简单的白噪声，它能够满足大部分的实际应用。对于海上或无需精密高度定位的场合，为了降低滤波器维数和计算量，有时可将天向通道中的加速度计随机常值偏值、速度误差和高度误差简化删去。但是，对于捷联惯导而言，如果导航运行过程中俯仰或横滚角机动变化较大，此时载体坐标系中的3个加速度计随机常值偏值容易从天向通道中辨识出来，高度通道不宜再作简化处理。

对于惯导而言，组合导航的主要目的在于通过速度或位置量测估计出失准角甚至惯性器件误差，提高惯导系统精度。在实际应用中，速度量测和位置量测之间是存在一定程度的信息冗余的。在高精度惯导中，失准角和惯性器件误差都比较小且稳定，需要较长时间才能估计出这些误差，通常有位置量测就可以，而不需要速度量测。在低精度惯导中，失准角和惯性器件误差都相对较大且不稳定，需要通过速度量测才能更快速地估计出这些误差，如果仅用位置量测则往往不能起到很好的误差抑制效果，除非实际位置测量噪声非常小。

不同导航系统之间进行测量参数或导航参数比对组合，往往都得考虑两者之间的时空不同步差异问题，即杆臂误差和时间不同步误差。根据实际情况，考虑是否将这些误差量增广列作Kalman滤波状态。

对于惯导与卫导之间的杆臂误差，建议事先进行准确测定，再作为已知参数设定并补偿，这样有利于降低滤波器维数。除非在某些特定的高精度应用场合，如机载定位定向系统(Position and Orientation System,POS)中，卫导天线安装在飞机蒙皮上方，惯导安装在机腹仪器仓内，难以准确测量出两者的相对位置，此时需将不能准确测定的杆臂误差列为状态。一般情况下，惯导精度等级越低或载体转动机动性越差，将越难通过滤波方法估计出杆臂误差，因而在低精度惯导中应当尽量事先给出准确的杆臂参数。

接下来分析时间不同步问题。通常导航计算机接收到惯性器件信号的延迟较少，而卫星信号从采集到定位解算再到传输给导航计算机，可能存在几十甚至一、二百毫秒的延迟，有些卫导系统延迟时间还不固定，这对高速飞行器的组合导航系统而言是十分不利的。有些情况下，时间延迟波动带来的误差远大于卫导本身的定位误差，例如飞机速度200m/s，延迟150ms将会引起30m的时间不同步定位误差，而卫导定位误差一般小于10m。如果卫导相对于惯导时间延迟基本固定不变，则可以作为状态进行滤波估计，在一定量级的载体加速度机动条件下是可观测的。如果时间延迟参数存在波动又难以准确估计，则应当优先考虑在组合导航系统硬件设计上做好同步处理和补偿，否则只能从软件算法上等效放大卫导的量测噪声(不好处理的非白噪声)，相当于降低了卫导的测量精度，必然会对组合导航系统造成负面影响。

显然，惯导的系统级标定或初始精对准过程均可视为一种特殊的组合导航算法，它们都以静止零速为参考速度，惯导解算速度即为速度误差观测量。系统级标定将惯性器件的漂移误差、刻度系数误差，以及器件之间的安装误差等待标定参数都扩充为滤波状态，通过一套完整的转动方案充分激励出所有误差源的影响，借助Kalman滤波器从导航速度误差量测中估计出待标定参数。精对准的主要目标在于从导航速度误差中估计出失准角，有时还能够估计获得部分惯性器件误差或抵消惯性器件误差的影响，例如多位置对准方法、单轴或双轴旋转对准方法等。

3 连续时间随机系统的离散化问题

在导航计算机上运算的Kalman滤波总是离散化形式的，针对如式(8)给出的连续时间随机系统，在使用Kalman滤波之前必须先进行离散化处理。随机系统离散化与确定性系统离散化最大的区别在于对输入噪声的等效处理。

简记Xk=X(tk)、Γk-1=G(tk-1)、Zk=Z(tk)和Hk=H(tk)，且记Ts为离散化间隔，对式(8)等效离散化，可近似得式(1)，其中

(10)

Qk-1≈q(tk-1)Ts

(11)

Rk≈r(tk)/Ts

(12)

在式(8)所示的量测模型中，v(t)为连续时间白噪声，r(t)为其功率谱密度，理论上连续时间白噪声的带宽无限，只是一种理想化的建模表示，这在实际系统中是不可能存在的。在实际应用中，大多数系统的量测方程是以离散形式直接给出的，如果在一定量测频率范围内量测噪声的方差大小基本不变，则在量测设备允许的条件下选用较高的量测频率对提高滤波估计精度是有益的；如果系统状态变化比较平缓，为了降低量测更新频率和减少计算量，则可将相继多次量测作平均处理，并相应减少量测噪声，利用平均量测进行Kalman滤波量测更新与进行高频率量测更新是基本等效的。例如在以速度为量测的扰动基座Kalman滤波初始对准中，反映失准角变化规律的惯导速度为缓变量，外界基座干扰为快变量，可利用一段时间内(如1s)的平均速度代替瞬时速度作为量测，这样可在减少基座干扰影响的同时降低量测更新频率。式(12)正说明了，在r(tk)为缓变或定值的情况下，若使用较长的量测离散化间隔Ts，则需要设置较小的噪声参数Rk。

4 噪声相关条件下的建模问题

在标准Kalman滤波中关于噪声的基本假设满足式(2)，如果不满足该条件，通常称其为噪声相关条件下的Kalman滤波，主要包括系统噪声与量测噪声相关、系统噪声为有色噪声、量测噪声为有色噪声三种情形[1-2]。

处理系统噪声与量测噪声相关的方法是将量测引入状态方程，从而消除相关性。事实上，系统噪声与量测噪声相关的实际系统例子非常罕见，即使相关也很难准确获得它们之间的相关性矩阵，因而该种情形往往只有理论上探讨的意义。

对于系统噪声或量测噪声为有色噪声的情形，通行的做法是对有色噪声建模，将其表示为白噪声激励的输出，即对有色噪声进行白化处理，再将有色噪声模型列入系统方程，构成增广系统。由于Kalman滤波采用状态方程描述系统，因而理论上只有能够用有限维状态方程描述的有色噪声才能作增广处理。实际应用时多将有色噪声近似成AR(p)模型(p阶自回归时间序列模型)，而且阶次一般不会太高，往往取1阶(至多2阶)就足够了[8]。

以惯导/卫导组合导航为例，常将陀螺漂移(加速度计零偏类似)建成如下误差模型

ε=εb+εr+εw

(13)

式中，εb为随机常值过程，εr为相关过程，εw为角速率白噪声，三者分别表示陀螺漂移误差中的不易变部分(随机常值)、缓变部分和快变部分。如果将惯性器件中相关过程都建模成AR(1)模型，则6个惯性器件(3个轴陀螺+3个轴加速度计)将增加6维状态，如果采用AR(2)模型则需增加12维状态，这对Kalman滤波器而言增加了很大的计算负担。从实用角度看，太复杂的惯性器件建模方法是不可取的，能大幅提高导航系统精度的结论也是不可信的[9-10]。在实际应用中，一般以“随机常值+白噪声”或“AR(1)相关过程+白噪声”建模就足够了，前者多用于器件误差变化时间相对于系统工作时间而言比较长(相对稳定)的情形，而后者主要用于误差变化时间相对较短的情形。

对于卫导量测噪声，建模为AR(1)也足够了，实际上卫导测量序列前后之间的相关性也很小，特别在量测间隔较大时(1s量级)，完全可近似为白噪声。同时，可将量测噪声矩阵当作对角阵看待，进而使用序贯Kalman滤波进行计算，避免了矩阵求逆运算，滤波结果与非对角阵量测噪声设置之间的差别不大。

另外指出，在导航中影响导航精度的主要因素是惯性器件的长时间相关误差项，而短相关或白噪声的贡献一般非常小，导航算法本身就是个积分过程，具有较强的高频噪声抑制能力。因而在导航算法前端应用数字滤波、小波滤波甚至神经网络滤波等手段进行去噪的做法是没什么意义甚至不可取的，降噪作用不大，反而有可能引起系统带宽变窄。前述所说的AR建模主要指的也是对相关时间较长项建模，而对于较短项完全可以忽略。

5 滤波快速计算问题

标准Kalman滤波算法的计算量(主要指浮点乘法运算次数)与状态维数的三次方成正比，状态维数的增加，使得计算量呈几何级数增长，高维系统的滤波对于导航计算机来说是一个沉重的负担，对于单片机之类的嵌入式系统而言尤其如是。

在运载体机动情况下，高精度的捷联惯导系统要求较高的惯性器件采样频率和导航解算频率。同理，在组合导航Kalman滤波中，为了获得高精度的惯导误差状态及其均方误差阵的传播，也需要较高的Kalman滤波时间更新频率。据分析，在组合导航Kalman滤波中状态均方误差阵预测的计算量最多(而非量测更新的矩阵求逆计算)，约占70%以上。针对状态转移矩阵Φk/k-1为稀疏矩阵的特点，常规算法存在大量的乘零运算操作，白白浪费了计算量，文献[11]提出了一种矩阵外积法，使得均方误差阵预测Pk/k-1所需的乘法次数降为s2-u2(其中s和u分别为状态转移矩阵中非零元素的个数和数值为1的元素个数)。更进一步地，文献[12]基于系统矩阵为稀疏矩阵、量测噪声方差阵为对角阵且量测矩阵的非零元均为1，以及惯导水平通道和高度通道弱耦合的特点，提出了均方误差阵预测直接展开计算，采用量测更新序贯滤波处理，以及将系统矩阵中次要元素强制置零和降维次优滤波等措施，极大地减少了Kalman滤波的乘法计算次数。直接展开计算法不仅减少了乘法计算量，还减少了用于程序循环控制的变量计算和比较次数，最终滤波运行速度与常规算法相比可提高1个数量级以上。当然，直接展开计算法的缺点是增加了程序的代码长度，典型的惯导/卫导组合算法程序约增加50k字节左右。

特别地，在低精度微机电系统(Micro-Electro-Mechanical System，MEMS)惯导/卫导组合中，根据物理含义，可以明显地知道某些系统状态分量之间是几乎无关的(例如陀螺随机常值漂移与杆臂误差之间)或弱相关的(例如失准角与位置误差之间、陀螺随机常值漂移与速度误差之间)，因而可以直接将它们之间的均方误差阵相应元素置零，无需计算，以节约计算量并减少程序代码。但需要注意的是，置零可能引起均方误差阵的非正定，这需要与均方误差阵下限设置技术结合使用，详见第11节。

实际系统中，捷联惯导算法的更新频率通常在100Hz以上，组合导航Kalman滤波的时间更新频率建议不低于10Hz。如果在一个惯导算法更新周期(10ms)内无法完成一次Kalman滤波时间更新，则可以将Kalman滤波时间更新分散成计算量大致相同的数小片(比如50小片)，每10ms仅计算其中的一部分(10小片)，这样在5个惯导算法更新周期(50ms)内便可完成一次Kalman滤波时间更新，同理也可将量测更新作同样的分散处理。这种分散处理的计算技术称之为时间分散Kalman滤波(Time-Distributed Kalman Filtering，TDKF)算法[13]。

6 平方根滤波问题

平方根滤波算法主要是针对均方误差阵更新过程设计，采用均方误差阵Pk的平方根进行循环更新，以减少数值表示的位数和减小计算误差。与标准Kalman滤波算法相比，平方根滤波计算只需要大约一半的有效数字位数就能达到同样的滤波精度，这在早期计算机位数不高时(单精度浮点甚至定点情形)是特别有效的。

下面以捷联惯导的天向失准角误差传播方程为例来说明Kalman滤波所需的有效数字位数。天向失准角误差方程为

(14)

常用的平方根滤波算法有Potter平方根滤波、奇异值分解(Singular Value Decomposition，SVD)滤波、UD分解滤波和信息平方根滤波等算法[1-2]。如果导航计算机仅支持单精度浮点运算，则需要考虑采用平方根滤波算法，并优先推荐使用UD分解滤波算法，而其他平方根滤波算法必要性不大。目前大部分计算机都支持双精度浮点运算，此时在组合导航Kalman滤波应用中则完全没有必要再采用平方根滤波算法，因为平方根滤波算法的计算量总是大于常规滤波算法的。

另外指出，表示导航参数比表示导航误差需要更多的有效数字位数。在惯导解算中，如果惯导速度的精度为0.001m/s，定位解算周期为T=0.01s，根据如下纬度更新公式可大致分析一下纬度表示所需的有效数字位数为

Lm=Lm-1+vN,m-1T/RMh

(15)

式中，不妨假设前一时刻的纬度取值Lm-1=1(rad)，由vN,m-1T/RMh≈10-12(rad)可知，为了准确得到当前时刻的纬度Lm，其表示需要12位有效数字，才会使得在大数与小数相加过程中小数不会被完全湮没。从以上计算分析过程看，适当延长导航更新周期T(即降低采样频率)可以在一定程度上减小数字表示位数，这对平台惯导系统而言是合适的，但对同时存在角运动和线运动的高动态捷联惯导系统来说是不可行的，因为在捷联惯导解算中为补偿不可交换误差，动态越大采样率要求就越高。

综上所述，无论从惯导解算还是从组合滤波角度看，除非作特殊处理，应当在硬件上选用具备双精度浮点运算能力的导航计算机。

7 自适应滤波与强跟踪滤波问题

理论上，只有在随机动态系统的结构参数和噪声统计特性参数都准确已知的条件下，标准Kalman滤波才能获得状态的最优估计。然而，实际应用中以上两类参数的获取都或多或少存在一些误差，致使Kalman滤波的精度降低，严重时还可能会引起滤波发散。不难理解，随机系统的模型误差往往会影响到其输出，换言之，量测输出中很可能隐含了关于系统模型(例如噪声参数)的某些信息，那么当系统模型参数不够准确时就有可能根据量测输出对部分参数进行自适应估计建模，这实质上属于系统辨识问题。

自适应滤波的方法很多，比较常见的一种状态空间模型描述为

(16)

(17)

其中，qk、rk、Qk和Rk为待自适应辨识的噪声参数。

文献[14]提出了一种自适应滤波算法，它在进行状态估计的同时还可以通过量测输出在线实时地估计系统的噪声参数。但笔者认为要同时对所有的噪声参数进行自适应估计一般是不可能的。实际上，系统噪声均值qk和量测噪声均值rk都可以增广等效为待估计的系统状态，通过可观测性分析判断能否进行自适应估计，如果不可观，则进行自适应滤波处理必然是无效的。因此，自适应滤波需要解决的主要问题是辨识系统噪声方差阵Qk和量测噪声方差阵Rk。系统噪声属于系统的固有特性，一般不易发生改变，即使有些变化，通常只要设置大致准确即可，相差3～5倍往往也不会对滤波估计结果造成太大的影响。在实际滤波应用中，微小的精度提高是次要的，系统工作的稳定性和可靠性才是更重要的，因而一般没必要对系统噪声作自适应处理。量测噪声主要由外部因素引起，容易发生变化且有可能变化较大，有必要采用自适应滤波处理，因而量测噪声方差阵Rk自适应滤波算法是实际中最常用的方法，其算法可表示为

(18)

(19)

Kalman滤波的噪声自适应效果与随机系统的可观测性密切相关。对于可观测性很强的系统，对部分噪声进行自适应处理是有可能实现的；但如果系统可观测性本就比较弱，还要进行噪声自适应就有可能比较困难了。在惯导/卫导组合导航中，根据位置或速度量测，要滤波估计陀螺漂移，特别是高精度的方位陀螺漂移，往往需要比较长的时间才会有估计效果，因此针对陀螺噪声的自适应处理基本上是不可行的。

强跟踪滤波也可以看作是一种自适应滤波方法，文献[15]给出了强跟踪滤波的理想特性：1)较强的关于模型参数失配的鲁棒性；2)较低的关于噪声及初值统计特性的敏感性；3)极强的关于突变状态的跟踪能力，并在滤波器达到稳态时仍保持这种能力；4)适中的计算复杂性。然而笔者认为强跟踪滤波存在理想是很理想的，但现实是很现实的问题，其在线自适应调整Kalman滤波增益使得输出残差序列正交的目标是完美的，但在实际应用中具体操作方法却是很难实现的，特别对于高维数的系统而言。此外，在强跟踪滤波中并没有考虑量测异常的影响，如果量测出现异常，则迫使状态跟随异常量测变化的思路是极不合适的。

在强跟踪滤波中，渐消因子的选取有单重和多重之分。单重渐消因子将所有量测和状态作为整体处理，使得所有状态总体上跟随量测的变化，即使没有突变的状态也会受到不良量测的影响。多重渐消因子考虑了不同状态之间的突变差异，分别分配以不同的比例系数，越不易变化的状态比例系数越接近1，越易变的越大于1，但多重渐消因子依然跟随量测的整体变化。为了更加细致地区分不同量测的影响，笔者曾提出了基于序贯滤波的多重渐消自适应滤波方法[1]。在强跟踪滤波中，若选取较少的渐消因子则求解简单，但也存在明显的缺陷；若渐消因子多，虽适应性强，但设计复杂，特别对于高维系统，要设计出合适的渐消因子参数矩阵是非常困难的，这是强跟踪滤波的一大弊端或不实用之处。针对强跟踪滤波的有效性验证，研究控制方面的文献选取的例子通常是比较简单的低维系统，可能容易产生效果[16]；但对于研究组合导航方面的高维例子，很多论文中展示的效果是读者难以复现的[17]，从而导致论文研究很多，实际应用却几乎没有。

组合导航强跟踪滤波性能与量测故障检测需求之间是相互矛盾的。强跟踪往往要求系统状态估计快速跟踪量测的变化，但量测快速变化有时是由故障引起的，虽然巨大变化的野值型故障容易被检测并被隔离掉，但稍微偏离正常值的量测就会让强跟踪滤波与故障检测算法无法同时满足，过分强调强跟踪性能必然损失故障检测能力，有引入错误的风险；而过分强调故障检测能力就会损失强跟踪性能。在实际应用中，总是以系统可靠性为首位的，状态一般不会发生突变，例如以高精度惯导的陀螺漂移估计为例，陀螺漂移对系统的影响微小，其估计过程必然是一个漫长的累积过程，不可能在短时间的量测突变中完成状态估计，如果因突变而产生错误的估计反而会对系统造成更大的负面影响。

8 联邦滤波问题

标准Kalman滤波的计算量与状态维数的三次方成正比，计算量因状态维数增加而急剧变大。一个复杂的大系统往往包含众多的状态变量，但大系统通常可以分解成若干子系统，并且在子系统中可能还存在一个关键的公共参考系统。例如惯导/卫导/里程仪/气压高度表组合导航系统就是这样的一个典型系统，它以惯导系统为主要参考导航系统(假设无故障)，其他三种导航子系统在正常工作时辅助惯导，以提高系统总体导航精度，当某一系统出现故障时将被监测和隔离，以免影响系统的总体性能。针对这类大系统，可以设计一个高维的综合滤波器，包含所有状态变量，再进行Kalman滤波，这一处理方式通常称为集中式滤波；也可以采取所谓的联邦滤波方法进行分散降阶处理，采用联邦滤波有利于降低各子系统的计算量，还便于各子系统的故障诊断和隔离，避免有故障的子系统影响整个滤波器，提高总体性能。

在联邦滤波中，如果所有子滤波器的状态均为公共状态，则全局状态融合精度与集中滤波器精度相同，结果均是最优的，且最优性与信息分配系数βi的选择无关。但是当某些滤波器存在私有状态时，联邦滤波的精度一般低于集中滤波，这时联邦滤波是一种次优滤波算法。

图1 联邦滤波示意图Fig.1 Schematic diagram for federated Kalman filtering

Carlson在提出联邦滤波时曾声称联邦滤波器已被美国空军的容错导航系统“公共卡尔曼滤波器”计划选为基本算法[19]，但从目前组合导航系统的发展情况看，历经30余年实践该算法并没有明显的优势，未获得广泛的实际应用。既然联邦滤波实际用途不大，那么针对信息分配系数的所谓最优性研究也就没什么意义了[20-21]。

针对目前常用的车载惯导/卫导/里程仪组合导航系统，或许是工程师们没有领悟到联邦滤波的精妙之处，据笔者所知并没有实际产品采用联邦滤波方案，而一般是采用三者的两两组合构成3个子组合导航系统，即惯导/卫导、航位推算/卫导和惯导/航位推算。在卫导信号有效时，惯导/卫导、航位推算/卫导组合工作，滤波估计惯导误差及里程仪与惯导之间的安装偏差角和里程仪刻度系数误差，以惯导或航位推算的输出作为系统输出，并没必要再将2个滤波器进行信息融合；在卫导信号短时间无效时，以纯惯导导航作为系统输出；在卫导信号较长时间无效时，惯导/航位推算工作，这时航位推算能够有效抑制惯导误差随时间发散，导航输出精度主要取决于航位推算精度。

9 非线性滤波问题

标准Kalman滤波仅适用于线性系统。对非线性系统作滤波估计，最常用和有效的方法是先进行泰勒级数展开，略去高阶项后近似为线性系统，再进行线性Kalman滤波。这种处理方法称为扩展Kalman滤波(Extended Kalman Filtering，EKF)，或称推广Kalman滤波。如果对非线性系统作泰勒级数展开并保留二阶项，则称为二阶Kalman滤波。不像确定性系统的二阶泰勒展开比一阶展开精度高一阶，即前者误差为O3而后者误差为O2，二阶滤波的精度并不明显比一阶EKF精度高，反而是对于高维系统而言，二阶滤波的计算量增加了许多，因此二阶滤波的性价比很低，并不实用。对于部分非线性系统，主要针对滤波时间更新过程，有时简单地采用降低更新周期的办法就能有效降低非线性的影响。

EKF采用解析方法进行一、二阶矩概率统计特性的近似传播，当系统非线性函数的雅可比矩阵求解比较复杂时，研究者们提出了利用采样点进行概率传播的滤波方法，例如采用确定性采样策略的无迹Kalman滤波(Unscented Kalman Filter,UKF)、中心差分Kalman滤波(Central Difference Kalman Filter,CDKF)，以及基于随机采样策略的粒子滤波(Particle Filter,PF)等[22]。对于高维系统而言，粒子滤波计算量巨大，目前还难以在嵌入式导航计算机中应用；UKF和CDKF也鲜有实际应用的实例报道。在研究和解决传统惯导/卫导组合导航问题时，为了引出非线性滤波而评价EKF因模型非线性或模型不准确会引起滤波发散，多数是存在夸张成分，或者没用好基础的EKF。

惯导系统的姿态(可用欧拉角、四元数或姿态阵表示)、速度和位置等导航参数满足如下非线性微分方程组

(20)

(21)

(22)

式中，各符号含义详见文献[1]。如果直接对上述导航参数状态作组合导航Kalman滤波估计，则称为直接滤波方法。直接滤波法并没有太大的优势，因为导航参数方程组始终是非线性的[23]。一般应建立如式(8)所示的导航参数误差方程，误差方程具有良好的线性特性，可以直接使用线性Kalman滤波估计。相对于直接滤波而言，利用误差方程进行组合导航Kalman滤波估计的处理方法称为间接滤波，在估计出导航误差参数之后，对计算导航参数进行校正便可获得准确的导航结果。在间接滤波中，对于惯性级惯导系统，失准角通常为角分量级、速度误差为1m/s量级、水平定位误差为1km量级，即使是低精度惯导系统，失准角误差最大为度量级，在短时间内(数十秒甚至几分钟)也可以将误差传播视为线性的。

基于速度误差或位置误差量测的惯导/卫导组合导航，速度和位置误差都容易保持为小量，且在3个失准角误差中由于天向重力的耦合作用，2个水平失准角的可观测性都很强，它们在很短时间内便可获得估计并校正使之成为小量，因而只有大方位失准角的非线性建模和滤波具有一定的必要性和实用价值。当失准角较大时，采用非线性建模和滤波方法，例如UKF或CDKF，即使滤波估计是二阶精确的，但三阶或更高阶误差总是被忽略的，换句话说，不论采取什么样的非线性滤波方法，虽说比线性Kalman滤波精度更高，但总是存在高阶截断误差。特别是在大失准角的情况下，如果仅仅采用输出校正方式将难以实现状态的最优估计。对于非线性滤波应用，为了提高估计精度，在滤波估计过程中宜采用状态反馈校正措施，使失准角逐渐减小，使误差传播转变为线性传播。在线性模型条件下，非线性滤波方法与线性Kalman滤波相比没有任何优势，反而是非线性方法的计算量通常更大。因此，在大失准角情况下使用非线性滤波的主要目的在于迅速估计出粗略的失准角，可以不将惯性传感器误差列入滤波模型以降低维数，滤波过程中当失准角降低至比较小时，从大失准角非线性滤波方式转到小失准角线性Kalman滤波方式会更加有效，降低计算量的同时还能达到最优滤波估计精度[24]。在实际应用中，绝大部分非线性组合导航问题都可以转化为线性方法解决，且线性滤波方法更加稳定可靠。当然，在某些对时间要求比较苛刻的初始对准中，如果采用非线性粗对准+线性精对准两段式对准的滤波时间太长，可以改用数据存储与逆向导航技术，有利于缩短对准时间和提高对准精度[25]。

10 可观测度分析问题

在现代控制理论中，确定性系统的可观测性是指由一段量测输出确定系统状态的能力，它属于定性的描述，对于某一状态或状态组合，要么可观测要么不可观测。定常系统的可观测性分析，可以使用可观测性矩阵进行判断，方法简单；但是时变系统的可观测性分析通常比较复杂，难以简单而有效地给出结论。

在Kalman滤波理论中，随机系统的可观测性概念与确定性系统略有区别，前者表示从一段量测中获得系统状态的无偏估计的能力。如果量测噪声阵正定，随机系统的可观测性与相应确定性系统的可观测性结论恰好一致。对于随机系统，仅仅进行定性的可观测性分析是不够的，显然，不可观测的状态分量肯定不会有滤波估计效果。但对于可观测的状态分量，即便获得了该状态的无偏估计(一阶矩)，然而其均方误差(二阶矩)还是存在大小差异的，均方误差可以看作是Kalman滤波估计精度的定量描述，它随时间的变化正体现了滤波器的收敛速度。

针对系统状态向量中的每一个分量Xk(j)(j=1,2,…,n)，定义它的可观测度如下[26]

(23)

由此可知，可观测度是针对某一状态分量在某一时刻而言的，其含义是某一状态分量的初始设置误差的标准差P0(jj)与同一状态分量在k时刻的滤波误差标准差Pk(jj)的比值。可观测度为无因次量，在数值上越大，表明在经过一段时间Kalman滤波后，相应状态分量的估计误差下降程度越显著，或者说精度提升效果就越明显。根据经验，可人为设置如下阈值大致判断状态分量Xk(j)的可观测度强弱

(24)

对于时变随机系统的可观测度分析，文献中常讨论的方法有分段线性定常系统(Piece-Wise Constant Systems，PWCS)方法[27-28]，通过分析系统的提取可观测矩阵(Stripped Observability Matrix,SOM)的奇异值或条件数，判断系统状态的可观测度[29-32]。实际上，那些方法是存在本质缺陷的，它没有考虑到系统噪声的影响，相当于把系统当作确定性系统看待，举一特殊的随机定常系统例子，如下

(25)

文献[33]给出的基于Kalman滤波均方误差阵Pk的特征值和特征向量的可观测度分析方法才是非常合理的方法，它充分考虑了系统所有噪声的影响，不只是量测噪声Rk、还有系统噪声Qk和状态先验噪声P0都会对Pk产生影响。实际上，Pk的对角线元素的平方根正好代表了各状态的滤波估计精度(均方误差大小)，这在Kalman滤波建模准确的情况下是必然成立的，如果实际应用时出现偏差，不应归咎于该分析方法不适用，而应重新审视建模是否准确。对于状态维数众多的系统，不同类型状态之间的物理含义是不同的，一般不能相互比较滤波估计精度，例如惯导/卫导组合的速度误差和定位误差之间，但同类误差之间是具有可比较性的，例如东向、北向和天向速度误差之间。若对均方误差阵Pk作特征值分解，特征向量往往表示一些状态的线性组合，它们不一定表示相同的物理量，因此通过比较特征值的大小来确定状态组合的可观测度，其物理意义不够明确。其实，最简单和最合适的方法就是直接选取Pk的对角线元素的平方根作为相应状态的可观测度，且同类物理量状态之间可直接进行比较。

最后指出，PWCS可观测度分析方法宣称无需滤波就能获得系统状态的可观测度，计算量小。事实上PWCS方法是粗略的，惯导/卫导组合的可观测度与载体具体的运行轨迹(机动状况与持续时间长短)密切相关，PWCS分段越少精度越差，有可能忽略了一些重要的运行细节，只有当PWCS分段时间无限短时(同于Kalman滤波时间更新周期)其结果才是精确的，与Kalman滤波均方误差阵分析方法一致(仅是不考虑噪声的情况下)，这时并不存在计算量上的优势。

11 均方误差阵边界限制问题

标准Kalman滤波是线性最小方差无偏估计，在系统建模准确的情况下，可以获得状态的最优估计。但是，在实际系统中，模型或多或少存在一些偏差，随着滤波的推进，长时间后有些状态的均方误差会逐渐变小，特别是对于不受或少受系统噪声影响的状态，例如随机常值状态(陀螺随机常值漂移或加速度计常值偏值)。均方误差变得很小，理论上表示所对应状态的滤波精度很高，但实际上由于建模误差或干扰影响，该状态不可能达到相应的估计精度，从而出现了理论精度与实际精度之间的矛盾。这一现象在惯导/卫导组合的天向通道中表现得尤为明显，由于天向存在重力耦合的影响，使得天向加速度计随机常值偏值的可观测度很高，短时间内就会获得很好的滤波效果，均方误差阵对应元素将快速收敛变得很小，但是，如果建模不准确，例如加速度计零偏会随温度缓慢变化，则均方误差过度收敛后滤波器将难以再适应状态的缓慢变化，产生滤波估计偏差。

解决滤波器均方误差过度收敛的一种方法是采用虚拟噪声注入技术，即人为加大系统噪声，使得由于噪声的不断激励作用，状态估计均方误差不至于过度收敛得太小。但加大系统噪声后，可能带来的不利后果是，如果长时间得不到量测更新，状态估计均方误差将变得很大，这也与实际情况不符。防止均方误差过度收敛的另一种直接而有效的方法是[13]，根据状态的实际物理含义或经验设置一定的均方误差下限边界Pmin限制(通常为对角阵)，当滤波器量测更新均方误差阵Pk的对角线元素小于Pmin对应下限值时，人为直接强制将其取为下限值，可用伪代码表示为

fori=1,2,…,n

ifPk(i,i)

Pk(i,i)=Pmin(i,i)

end
end

这里一般不必考虑Pk非对角线元素的影响，总可保证Pk是对称正定的。当然，均方误差下界也不能设置得过大，否则会影响Kalman滤波估计精度，造成状态波动。

类似地，也可以采用均方误差阵上限限制措施，防止可能出现的滤波异常。只是上限边界限制不能再简单采用直接设置对角线元素的方法，因为这可能导致Pk不正定，而可采用如下方法

end

其中，Pmax为人为设置的均方误差上限边界对角阵，上述伪代码式的含义是，当对角线均方误差Pk(i,i)超过上限Pmax(i,i)时，将Pk的第i行和第i列元素都同时缩小s倍，显然，也总能保证Pk是对称正定的。

12 结论与建议

标准Kalman滤波基本公式在理论上非常完美且不复杂，在随机系统建模理想准确的条件下，能够得到系统状态的最优估计，即线性最小方差无偏估计。但在实际组合导航应用中，系统建模或多或少存在误差，也就使得实际应用时不存在理想最优的前提条件，研究者和工程师们在关注滤波估计精度的同时，更应关注实际应用时的滤波稳定性和可靠性。

导航技术是一门工程实践性很强的技术。从技术传承和工程应用的角度看，特别在组合导航的非线性滤波、强跟踪滤波和联邦滤波等几个方面，建议热心的研究者们在发表论文时能给出仿真部分的数据和代码，供他人学习参考或改进，这样才更有利于促进群体理论研究水平的提升和成果转化。遗憾的是，目前许多论文的仿真结果是不可复现的，浪费了大量科研经费和读者的时间精力，迫使读者也即后继的研究者又提出了看似效果更好的不可复现的所谓新方法，如此不断反复，理论和仿真研究似乎非常完美深入，但却鲜有实际应用。

不妨将经典Kalman滤波与经典控制领域的比例-积分-微分(Proportianl-Integral-Derivative,PID)控制做个类比。现代控制理论虽然取得了很大的发展，解决了许多经典控制理论不能解决的问题，但经典PID控制仍然应用最为广泛，其原因在于：1)结构简单、鲁棒性和适应性好；2)大多数控制对象使用常规PID即可满足实际需求；3)调节整定很少依赖于系统的具体模型；4)各种高级控制方法在应用上还不完善，难以被技术人员掌握。笔者认为上述描述也非常适合于传统Kalman滤波与许多高级滤波方法的对比。研究者们在提出和仿真新滤波方法时往往过分强调了新方法的优势，而忽略了传统Kalman滤波方法通过细节上的简单处理也可能实现同样的效果，例如采用系统状态反馈、状态估计均方误差阵(P)的方差限制或重置、系统噪声方差阵(Q)的虚拟建模、量测噪声方差阵(R)的自适应处理等措施。可以说，在许多场合仅需对PQR参数进行简单调整，就能应用好组合导航Kalman滤波技术，取得良好的估计效果。

致谢感谢以下同行对论文初稿的审阅并提出宝贵的修改意见：付强文、翁浚、王茂松、张全、杨君、王文举、匿名评审专家。