捷联惯性导航系统中的卡尔曼滤波方程

时间：2023-06-23 理论教育版权反馈

【摘要】：但是捷联惯导计算的测量信号为物体运动产生的惯性信号，信号随物体运动而变化，测量信号本身具有随机性质，因此当前时刻的期望响应信号是未知的，目前只能采用基于状态空间模型的卡尔曼滤波方法。目前，研究的热点主要是将其与各种组合导航算法相结合的联邦卡尔曼滤波方法、提高解决非线性问题的计算精度和效率的各种卡尔曼滤波改进算法。因此下面主要讨论离散系统的卡尔曼滤波，即用离散系统递推线性最小方差估计。

捷联惯性导航系统中的卡尔曼滤波方程

1.卡尔曼滤波原理

利用滤波器可以从包含噪声的测量数据中抽取期望得到的信号。现代滤波理论表明，滤波方式可以通过时间域、频率域或空间域实现，实现的结果为求解出期望的响应信号。但是捷联惯导计算的测量信号为物体运动产生的惯性信号，信号随物体运动而变化，测量信号本身具有随机性质，因此当前时刻的期望响应信号是未知的，目前只能采用基于状态空间模型的卡尔曼滤波方法。

卡尔曼滤波实质是一种线性均方误差最小的估计，另外也有最大似然估计的方法，它是由计算机实现的实时递推算法，由卡尔曼（R.E.Kalman）于1960年提出。这种滤波方法是从与被提取信号有关的观测量中通过算法估计出所需信号，使估计的信号与期望信号的均方误差最小。将信号输出过程视为白噪声作用下的一个线性系统的输出，用状态方程描述这种输入输出关系。估计过程中，系统状态方程、观测方程和白噪声激励（系统状态噪声和观测噪声）的统计特性构成滤波算法。由于所利用的信息都是时域内的变量，所以不但可以对平稳的、一维的随机过程进行估计，也可以对非平稳的、多维的随机过程进行估计，具有较好的应用性，被广泛应用于导航、信号处理等领域。目前，研究的热点主要是将其与各种组合导航算法相结合的联邦卡尔曼滤波方法、提高解决非线性问题的计算精度和效率的各种卡尔曼滤波改进算法。

卡尔曼滤波对一维或多维的离散时间状态X进行估计，处理的是随机噪声，要求模型精确，随机干扰信号统计特性已知。

假设动态系统的一阶线性动态方程和测量方程为

X（t）=F（t）X（t）+G（t）W（t）（7-89）

Z（t）=H（t）X（t）+V（t）（7-90）

式中，X（t）为系统的状态矢量（n维）；F（t）为系统的状态矩阵（n×n阶）；G（t）为系统的动态噪声矩阵（n×r阶）；W（t）为系统的过程白噪声矢量（r维）；Z（t）为系统的测量矢量（m维）；H（t）为系统的测量矩阵（m×n阶）；V（t）为系统的测量噪声矢量（m维）。

其中卡尔曼滤波要求系统噪声矢量W（t）和测量噪声矢量V（t）都是零均值的白噪声过程。

工程对象一般都是连续的系统，因此对系统的状态估计可以按连续动态系统的滤波方程进行计算。然而在实际应用中，常常是将系统离散化，用离散化后的差分方程描述连续系统。因此下面主要讨论离散系统的卡尔曼滤波，即用离散系统递推线性最小方差估计。为此将状态方程式（7-89）和测量方程式（7-90）离散化，可得

式中，

T为迭代周期；X_k为系统在k时刻的n维状态矢量，也是要求的被估计矢量；Z_k为在k时刻的m维测量矢量；Φ_k，k-1为k-1时刻到k时刻的系统状态转移矩阵（n×n阶）；H_k为k时刻的测量矩阵（m×n阶）；W_k-1为k-1时刻的系统噪声矢量（r维）；Γ_k-1为系统噪声矩阵（n×r阶），它表示由k-1时刻到k时刻的各个系统噪声分别影响k时刻各个状态的程度；V_k为k时刻的m维测量噪声矢量。

根据卡尔曼滤波要求，假设{W_k，k=0，1，2，…}和{V_k，k=0，1，2，…}是独立的均值为零的白噪声序列，即有

式中，符号E{}表示取均值；δ_kj为Kroneckerδ符号；Q_k为系统噪声方差阵，由于并非系统的所有状态变量x_i均有动态噪声的缘故，故Q_k是一个非负定矩阵（n×n阶）；R_k为测量噪声方差阵，由于每个测量值Z_i均含有噪声，故测量方差阵是一个正定矩阵（m×m阶）。

式（7-95）～式（7-97）中，Q_k、R_k和Q（t）、R（t）的关系可近似表示为

同时又假设系统的初始状态X₀也是正态随机矢量，其均值和协方差矩阵分别为

对于一个实际的物理系统而言，现在和未来时刻的干扰绝对不会影响系统的初始状态X₀；为了简化问题的讨论，可以认为系统的初始状态X₀、系统的噪声W_k、测量噪声V_k是相互独立的，即对所有的k=0，1，2，…有E{X₀W^T_k}=0；E{X₀V^T_k}=0和E{W_kV^T_k}=0。

以上内容给出了离散系统的数学描述以及有关噪声的概率统计特性的假设。现在先讨论最小方差估计，如果给定测量数据{Z_j，j=1，2，…，k}之后，求状态矢量X_i在某种意义下的最优估计，记做。根据观测时刻k与待估计矢量X_i所在的时刻i的关系，将统计估计分为三类：若i＞k，称为X_i的预测估计，即由以前的观测数据预测未来的状态矢量。若i=k，称为X_i的滤波估计，它是“实时动态估计”。若i＜k，称为X_i的平滑估计，又称内插估计。

最小方差估计的定义是：如果估计使得

成立，则称为矢量X_i的最小方差估计。

记为估计误差，为估计误差的协方差矩阵。如果，则称为X_i的无偏估计。如果为{Z_j，j=1，2，…，k}的线性函数，则估计就称作矢量X_i的线性估计。

实际工程应用中常常希望由t_k时刻的观测值Z_k计算得到该时刻的状态X_k的估计。对于动态系统，由于X_k是从t_k以前时刻的状态按照系统转移规律变化过来的（含噪声的影响），现在时刻的状态和以前时刻的状态存在着关联，所以利用t_k时刻测量值Z_k进行估计，必定有助于估计精度的提高。但是对于线性最小估计方差来说，由于计算方法的限制，若采取同时处理不同时刻的全部测量值来估计t_k时刻的状态X_k，计算工作量相当大，因此这种估计方法不适合实时估计动态系统的误差。如果无须“同时全部”处理t_k时刻前的测量数据，而是采取一种将前后时刻“关联”起来的递推算法，问题就可解决。这就是卡尔曼在线性最小方差估计的基础上，提出的递推线性最小方差滤波估计——卡尔曼滤波。卡尔曼滤波是一种递推数据处理方法，它利用上一时刻t_k-1的估计和实时t_k时刻的观测值Z_k进行实时估计得到。由上一时刻t_k-1的估计是使用再上一时刻t_k-2的估计和t_k-1时刻的观测值Z_k-1得到的，依此类推，可以一直上溯到初始状态矢量和全部时刻的观测矢量{Z₀ Z₁ Z₂ … Z_k-1}。所以这种递推的实时估计，实际上是利用了所有全部测量数据而得到的，而且一次只处理一个时刻的测量值，使计算量大大减少。递推线性最小方差估计的估计准则仍然符合式（7-100）。它的估计同样是测量值的线性函数，而且估计也是无偏估计。

2.离散系统的卡尔曼滤波方程

现在的问题是：在给定测量数据{Z_j，j=1，2，…，k}后，如何寻找到状态矢量Xk的递推线性最小方差滤波估计。为了便于理解，这里介绍一种推倒卡尔曼滤波方程的比较直观的方法。关于数学上的严格讨论，请参阅有关离散时间系统递推估计的资料。假设已知k-1时刻和此时刻以前的测量数据{Z_j，j=1，2，…，k-1}，通过计算得到了状态矢量X_k-1的线性最小方差无偏估计。根据方程式（7-91）可以看出，由于随机干扰矢量W_k-1是一个不能预测的随机矢量，因此只能用下述方程：

来计算系统状态矢量X_k的一步估计值，一步估计值的误差记为，有，当是状态矢量X_k-1的最小方差滤波估计时，就是状态矢量X_k的最小方差预测估计。方程式（7-101）称为状态一步预测方程。

观测方程式（7-92），按照同样的推断，由上述系统状态矢量X_k的一步预测估计得，即

现在在时刻k又测量到测量矢量Z_k，当前的实际测量矢量Z_k与式（7-102）计算获得的Z_k的一步预测估计

之间存在着误差，记作1。将式（7-92）表达的Z_k的测量值和一步预测估计式（7-102）代入表达式中，可以得到测量矢量Z_k的一步测量误差（简称残差），即

根据式（7-103）的右端可以看出，Z_k的一步预测误差由两部分构成：一部分是由状态矢量X_k的一步预测的误差，以的形式出现；另一部分是测量本身的噪声V_k。对于测量误差中的前一成分，自然会想到用它去修正状态矢量X_k的一步预测估计，以便预测X_k的滤波估计。

实际上，在数据处理过程中，由于k时刻可以测量到Z_k，而是由上一步的式（7-102）计算得到的，因此可以获得的信息只是残差，只是残差中的主要成分，无法具体算出。为了实时估计状态X_k，只有用残差去近似的一步预测估计值的误差X，从而实现的修正，获得滤波估计。是在的基础上估计X_k所需要的信息的，故称为新息（Inovation）。在线性估计范围内，一般总是采用加权的方法来修正一步预测估计，于是可用式（7-104）来计算X_k的滤波估计：

式中，K_k为滤波增益矩阵（n×m阶）。

式（7-104）称为状态滤波方程。根据以上分析，式（7-104）中右端第一项是由k-1时刻和以前所有时刻的测量矢量得到的，第二项中的新息含有k时刻的测量矢量Z_k，由此可以认为状态X_k的递推滤波，是由k时刻及其以前各时刻的测量值{Z_j，j=1，2，…，k}计算得到的。现在的问题是如何确定增益矩阵K_k。

增益矩阵K_k具有最优加权的含义，即在式（7-104）中，K_k的取值应使卡尔曼滤波对于状态X_k的估计误差X为最小。换句话说，由于增益矩阵K_k的取值，使得最小方差估计准则，即式（7-100）得到满足。

由式（7-91）表示的X_k和式（7-101）表示的可以导出：

根据协方差的定义，以及式（7-105），有

同理，可以获得

将式（7-105）代入式（7-107），可得

将式（7-109）的右端展开，通过整理可得

现在，根据式（7-110）来分析增益矩阵K_k应该如何取值，才能使估计误差的协方差矩阵P_k最小。根据前述假设，R_k为测量噪声方差阵，它是一个对称正定矩阵，而H_kP_k|k-1H^T_k至少是非负定矩阵，因此H_kP_k|k-1H^T_k+R_k必定为正定矩阵。根据正定矩阵的性质可知，存在一个可逆矩阵S_k，使得

S_kS^T_k=H_kP_k|k-1H^T_k+R_k （7-111）

将式（7-111）代入式（7-110），推导可得

P_k=P_k|k-1-K_kH_kP_k|k-1-P_k|k-1H^T_kK^T_k+K_kS_kS^T_kK^T_k （7-112）

观察式（7-112）右端，显然它是一个关于矩阵K_k的二次多项式，运用一般的二次多项式的配方法则，可以证明存在一个矩阵D_k使得下面的等式成立：

P_k=P_k|k-1+（K_kS_k-D_k）（K_kS_k-D_k）^T-D_kD_k^T （7-113）

比较式（7-112）与式（7-113）可得

D_k=P_k|k-1H^T_k（S^T_k）^-1 （7-114）

由于矩阵P_k|k-1是一步预测估计误差的协方差阵，因此矩阵P_k|k-1及矩阵D_k均与增益矩阵K_k的选取没有关系。于是从式（7-113）可以看出，若使矩阵P_k满足最小条件，即式（7-100），则必须有如下等式：

K_kS_k-D_k=0 （7-115）

将S_k和D_k代入式（7-115），有

K_k=P_k|k-1H^T_k（H_kP_k|k-1H^T_k+R_k）^-1 （7-116）

若使矩阵P_k达到最小，即它所属的二次齐次式达到最小，也即方阵P_k的迹（trace）：达到最小。所以，根据上述的讨论，K_k根据式（7-116）确定

以后，则P_k=min，增益矩阵K_k是最优增益矩阵，于是，由式（7-104）得到的状态矢量X_k的估计就是最小方差线性滤波估计，或称为卡尔曼滤波。(www.xing528.com)

在滤波开始时必须有初值和P₀才能进行，因此必须选择给定和P₀。为了确保卡尔曼滤波的无偏性，假设初始（t=0）条件为，则

成立。无偏条件应满足：

利用数学归纳法可以证明，只要系统状态矢量在初始时刻（k=0）的卡尔曼滤波是无偏的，那么卡尔曼滤波值在任何时刻（t=k）也必定是无偏的。

根据以上对状态矢量X_k的最小方差线性递推估计过程的分析，可以得到如下一组递推滤波方程组，通常称为离散系统卡尔曼滤波方程。

状态一步预测方程：

状态估计方程：

最优滤波增益方程：

K_k=P_k|k-1H^T_k（H_kP_k|k-₁H^T_k+R_k）^-1 （7-121）

一步预测均方误差方程：

P_k|k-1=Φ_k，k-1P_k-1Φ^T_k，k-1+Γ_k-1Q_k-1Γ^T_k-1 （7-122）

估计均方误差方程：

P_k=（I-K_kH_k）P_k|k-1 （7-123）

或：

P_k=（I-K_kH_k）P_k|k-1（I-K_kH_k）T+K_kR_kK_k^T （7-124）

由上述式（7-119）～式（7-124）确定的系统称为卡尔曼滤波器，它表现为计算机的数据处理——最小方差线性递推估计运算。卡尔曼滤波器的输入信息是系统的测量输出Z_k，滤波器的输出则是系统的状态矢量X_k的最小方差线性无偏估计。卡尔曼滤波方程中的前四个方程，即式（7-119）～式（7-122）包括了由输入量测量值Z_k到计算输出值的计算过程。估计方差P_k的式（7-123）或式（7-124）在计算下一步预测方差时是必不可少的。利用式（7-123）和式（7-115）均可以求得P_k。显然前者的计算量小，但是在计算机有舍入误差的情况下不能保证计算出的均方差矩阵P_k一直保持对称。而后者计算量较大，但可以保证P_k为对称阵。故在设计卡尔曼滤波器时，可以根据系统的具体要求来选择其中一个方程。根据卡尔曼滤波方程，即式（7-119）～式（7-123）可以给出离散系统卡尔曼滤波方程计算程序功能图，如图7-11所示。

图7-11 离散系统卡尔曼滤波方程计算程序功能图

由图7-11可以看出，由测量数据Z_k计算状态估计时，除了需要知道描述系统测量值的矩阵H_k和状态转移矩阵Φ_k，k_-1以及噪声方差矩阵Q_k和R_k，还必须有前一步计算的状态估计和估计均方差P_k-1。若计算出了k时刻的和P_k之后，则又可以使用它们计算下一步（t=k+1）时刻的和P_k+1。因此，由初值和P₀开始计算和P_k是一个循环递推的过程。

此外，倘若在系统和测量值中，含有已知的确定值输入量时，系统状态方程和测量方程分别表达为如下形式：

X_k=Φ_k，k_-1X_k-1+Γ_k-1W_k-1+B_k-1U_k-1 （7-125）

Z_k=H_kX_k+V_k+Y_k （7-126）

式中，U_k-1为确定性输入矢量，或称s维控制矢量；B_k-1为系统输入矩阵，或称n×s阶控制系数矩阵；Y_k为测量值中确定性输入矢量（m维）。

由式（7-125）和式（7-126）所构成的系统，其卡尔曼滤波方程中，将状态矢量一步预测方程式（7-119）改为

将状态估计方程式（7-120）改为

其他最优滤波增益方程、预测均方误差方程和估计误差方程不变。

3.转移矩阵Φ_k，k_-1和系统噪声方差矩阵Q_k的计算

离散系统卡尔曼滤波方程的显著优点就是方程的递推性，利用计算机这一有力的工具进行运算就能实现滤波。而工程中的系统很多都是连续系统，为了实现在计算机中进行滤波计算，常常将连续系统离散化。连续系统离散化的实质，就是根据连续系统的状态矩阵F（t）计算出离散系统的转移矩阵Φ_k，k-1，以及根据连续系统的系统噪声方差强度矩阵Q（t）计算出离散系统噪声方差矩阵Q_k。

动态系统状态方程式（7-89）的齐次方程为

方程式（7-129）的齐次解为

X_k=Φ_k，k-1X_k-1 （7-130）

式中，Φ_k，k_-1为从历元t_k-1到历元t_k的状态转移矩阵。

假设计算周期T远远小于系统状态矩阵F（t）发生明显变化所需要的时间，可将F（t）近似看成定常矩阵，则根据定常系统的齐次解有

一般情况下，在递推计算过程中，由历元t_k-1到历元t_k的时间间隔为计算周期T，将Φ_k，k-1在t_k-1处展开成泰勒级数，有

由式（7-131）可看出，状态转移矩阵Φ_k，k-1由无穷多项构成，为了减小截断误差，理应取尽可能多的项数之和。然而，在实际计算中，项数取得过多，不仅会大大增加计算工作量，而且由于计算步骤增多，反而导致计算的误差增大。因此，求和的项数要取得合适。为此，一种方法是在转移矩阵计算程序中，预先设置一个整数m，取10^-m作为一个阈值，当完成前L项累加时，将这L项之和与第L+1项比较，若第L+1项的值与前L项之和的比之小于阈值10^-m，则停止累加。另外，也可通过滤波器设计过程中的误差分析结果来确定合适的取数。若将滤波器的计算周期T平均分成N个时间间隔ΔT（即ΔT=T/N），滤波周期T与计算步长ΔT的关系示意如图7-12所示。