2.4 动态系统中的状态估计算法

第2.2和2.3节介绍的是一般的参数估计问题，而航天器自主导航系统中要估计的是动态系统中的状态，其随时间演化的方程由轨道动力学给出，因此在本节中对动态系统的估计算法进行介绍。

2.4.1 递归贝叶斯估计算法

图2-1给出了一阶隐式马尔科夫模型，在该模型中，状态量x不能直接观测到；z为x的函数，且能够被观测到，称为观测量。

一阶隐式马尔科夫模型具有如下属性：

属性1：在给定tk-1时刻的状态xk-1的前提下，tk时刻状态xk（以及未来时刻的状态xk+1，xk+2，…）出现的概率和tk-1以前时刻的状态、观测量条件独立，也即

式中，。

同样，在给定当前时刻状态的前提下，过去的状态和未来的状态、观测量独立，也即

属性2：当前时刻的观测量仅依赖于当前时刻的状态量，和其他时刻的状态条件独立，也即

定理2.4：递归贝叶斯估计算法给出在获得了tk时刻以及tk时刻以前的所有观测数据的前提下，递归地计算的方法。具体如下：

（1）初始化：给出初始时刻的状态先验概率密度函数p（x0）。

（2）预测：给定动力学模型，在tk时刻xk预测的概率密度函数由Chapman-Kolmogorov公式计算得到，即

（3）更新：在tk时刻，给定观测量zk，则xk的后验概率密度函数可以由贝叶斯法则给出，即

式中，，为归一化的常数。

证明：

首先由贝叶斯法则可以得到

式（2-63）右边的分子可以写作

式（2-64）利用到了如下公式

利用马尔科夫属性2，式（2-64）可以写作

式（2-63）的分母可以写作

将式（2-65）和式（2-66）代入式（2-63）可以得到

在式（2-67）中为归一化的系数，和测量方程对应，为更新过程；由下式给出

式中和状态预测过程相对应。在推导该式的过程中，利用了马尔科夫属性1 [式（2-58）]。

式（2-67）给出了马尔科夫动态系统贝叶斯估计的一般算法，观察式（2-67）和式（2-68）可以知道为的函数，因此可以用定理中的递归形式进行计算。

2.4.2 卡尔曼滤波算法

上一小节中给出的一阶马尔科夫动态系统递归贝叶斯估计算法，对于一般的系统很难得到解析解，如果要得到解析解，必须做出一些假设。本节中将介绍的卡尔曼滤波（Kalman Filter，KF）就是在对动态模型和观测模型做出特定假设基础上的一种解析的递归式贝叶斯估计算法。KF由卡尔曼（Kalman）于1960年首次提出，它是一种最优的递归式数据处理算法。由第2.2节可知，存在很多种最优判据，而在给定假设的前提下，无论是哪种最优判据KF均为最优。KF是将所有可用的测量数据、系统和测量设备的先验信息组合在一块对待估状态进行估计，可使得误差在某种统计意义下最小。尽管如此，KF不需要对以前时刻的所有测量数据进行存储，并在有新测量数据的时候进行重新处理，这一特性对于滤波算法的实际实施至关重要。

相对于仅考虑测量方程的最小二乘算法而言，KF的优势是能够实现对动态变化状态变量的估计；相对在频域空间设计的维纳滤波算法而言，KF使用状态空间法在时域空间设计，所采用的递推形式便于在计算机上实现。由于上述特点，KF理论一提出便立即受到工程界的重视，阿波罗飞船导航系统的设计是KF算法早期应用的成功案例。目前，KF理论作为一种重要的最优估计理论，被广泛地应用于各个领域，尤其是基于多源信息融合的组合导航系统的设计。

考虑如下的线性定常系统

式中，x和z分别为状态和观测量；Fk,k-1为tk-1时刻至tk时刻的状态转移矩阵；Hk为测量矩阵；wk为系统噪声；νk为测量噪声。噪声wk和νk均为高斯噪声，均值为0，并且满足以下条件

式中，Qk为系统噪声方差阵；Rk为测量噪声方差阵；通常假设Qk和Rk为正定阵。

此外，状态的先验概率分布满足，其中代表高斯概率密度函数（见附录D5.1）。

式（2-69）的概率密度模型可以写作（证明参见附录D5.2节）

定理2.5：考虑如式（2-69）所示的系统模型，则定理2.4中的贝叶斯滤波方程具有解析的表达式，且

上述分布的参数由下面的滤波预测方程和更新方程给出。

滤波预测方程为

KF更新方程为

其中

在证明这一定理前，首先给出联合高斯概率密度函数的两个重要属性（具体推导参见附录D5.2）：

属性1（由联合概率密度函数到条件概率密度函数）：如果x和z满足联合高斯分布，则仍为高斯概率密度函数，并由下式给出

其中

属性2（由条件概率密度函数到联合概率密度函数）：对于线性方程

如果

且。记，则

接下来证明定理2.5，证明的主要流程如图2-2所示。

具体步骤如下：

假设

由马尔科夫属性1和式（2-70）第一式可以得到

式（2-80）和式（2-78）对应，式（2-81）和式（2-79）的第一式对应，则由式（2-79）的第二式可以得到

从而可以得到

记

则式（2-83）可以写成式（2-71）第一式的形式，其中

由式（2-79）的第二式和式（2-70）可以得到

再由式（2-75）可以得到

其中

记

则可以得到式（2-73）。从而得证。

应当指出的是，证明的过程中利用了假设式（2-80），这一假设可以利用归纳法证明成立。

注：

（1）根据MMSE估计的最优判据可知，Pk为最优估计。

（2）KF更新方程有着几种不同的形式，参见表2-2（与LMMSE估计的表2-1对应）。

（3）如果初始状态为任意分布，均值为、方差为P0；过程噪声wk是均值为0、方差为Qk的独立过程；测量噪声νk也是均值为0、方差为Rk，且初始状态、过程噪声、测量噪声相互独立，则定理2.5给出的KF是最优线性无偏估计。

2.4.3 扩展卡尔曼滤波算法

Kalman最初提出的滤波基本理论只适用于线性系统，并且要求测量方程也必须是线性的。但是，在工程实践中遇到的物理系统，其数学模型往往是非线性的，如在航天器自主导航技术研究中经常用到的轨道动力学方程是非线性的，天文自主导航系统中对应的视线方向测量方程也是非线性的。为了将KF算法推广用于非线性系统，在KF基本理论提出后的10多年时间里，Kalman、Bucy和Sunahara等学者致力于研究KF在非线性系统中的应用，提出了扩展卡尔曼滤波（Extended Kalman Filter，EKF）算法。

EKF算法设计的基本思路是通过截取系统方程和测量方程中非线性函数泰勒级数展开式的一阶项，对系统进行线性化，然后将KF方程用于线性化模型以获得状态估计值。EKF算法是针对非线性系统进行状态估计最常用的算法，如美国的“星尘（STARDUST）”号探测器采用EKF处理光学成像测量信息，从而确定探测器的位置。基于EKF的姿态确定方法广泛应用于高精度卫星姿态控制系统，如美国的哈勃太空望远镜（H ST）和日本的先进陆地观测卫星（ALOS）等。

用于EKF算法研究的非线性系统模型为

式中，xk∈ℝn为状态变量；zk∈ℝm为观测量；f: ℝn→ℝn为状态转移函数；h: ℝn→ℝm为测量函数；wk和νk均为零均值白噪声，并且满足以下条件

对于式（2-89）所示的非线性系统，很难找到一种严格的递推滤波方法，通常采用近似方法来处理非线性滤波问题，应用比较广泛的近似方法是非线性系统的线性化。为了针对非线性系统应用KF方程，作如下基本假设：状态变量预测值与实际值之间的差能够用一个线性方程表示，该线性方程能够足够准确地对滤波器的实际误差特性给予描述。这个基本假设在工程实践中往往可以得到满足，描述预测值与实际值之差的线性方程称为线性干扰方程。常用的EKF算法是针对线性化后的模型设计的。

EKF算法方程描述如下，定义预测误差为

式中，表示状态变量的预测值。线性干扰方程的形式为

式中，称为雅克比矩阵；Δzk=。

在线性干扰方程的基础上，仿照线性KF基本方程，不难给出对偏差Δxk进行估计的KF方程为

值得注意的是，在应用过程中，往往不对偏差Δxk进行预测，而是直接设置状态偏差的一步预测值，此时

一般通过系统方程式（2-89）的第一式对状态变量预测，即

利用估计得到的状态偏差Δ^xk对预测值^xnk或^xk|k-1进行修正，得到

为了精确实现对状态变量的预测，针对式（2-95），人们提出了多种改进方法，如数值积分、求解微分方程等。相应的，为了提升状态变量修正的效能，针对式（2-96），也有不同的实现方法，如求解非线性优化问题等。在实际应用过程中，应综合考虑系统非线性程度、系统噪声和测量噪声影响的权重、精度要求，以及计算量等因素，设计或选择适当的状态预测和更新方法。在系统动态变化不明显或呈现周期性变化规律时，可采用常增益或周期增益滤波器，避免了对滤波增益阵Kk的递推解算，能够降低计算量、减小滤波算法的复杂程度。类似算法广泛用于基于陀螺和星敏感器的卫星姿态确定系统。

2.4.4 无迹卡尔曼滤波算法

1．问题描述

20世纪60年代提出的EKF将KF技术推广用于非线性系统，其基本思路是通过截取状态方程和测量方程中非线性函数泰勒级数展开式的一阶项，对系统进行线性化，然后将KF方程用于所获得的线性化模型以获得状态估计值。EKF算法历史悠久，原理直观，已被用于航天器定轨和飞行器导航等多个领域。EKF算法的主要问题在于，在线性化过程中忽略了非线性函数泰勒级数展开式的二阶项和其他高阶项；在初始误差较大或系统受到外界干扰的情况下，有可能引入较大的线性化误差，从而降低滤波精度。为了解决这一问题，人们提出了二阶滤波等改进方法，试图通过引入非线性函数泰勒级数展开式的高阶项来改善滤波性能。虽然采用二阶滤波可以提高滤波精度，但对于复杂的非线性系统而言，计算非线性函数的高阶导数往往比较困难。另外，基于贝叶斯估计理论和蒙特卡洛方法的PF算法也可以解决非线性系统的状态估计问题。但是，应用PF算法需要对大量粒子进行预测和更新，对于性能有限的星载计算机而言，计算负担较重。

20世纪90年代，关于无导数（Derivative Free）非线性滤波算法的研究取得了较大发展，其中比较著名的包括UKF、二阶插值滤波和容积卡尔曼滤波（Cubature Kalman Filter，CKF）算法等。UKF和二阶插值滤波算法分别采用无迹变换和二阶插值技术描述非线性函数泰勒级数展开式的二阶项和其他高阶项，基本思路是通过确定性方法选择少量样本描述状态变量的均值和方差特性，然后对各个样本分别进行非线性变换，并将它们通过非线性系统方程和测量方程后的样本均值和样本方差分别作为状态预测值、测量预测值和相应方差阵。UKF可以达到与二阶滤波相似的精度，而不必对非线性函数进行求导，应用更简便且其计算量与EKF处于同一个数量级，不会显著增大计算负担。CKF是近年来提出的一种新型非线性滤波算法，该算法基于球面径向规则设计，经过严格的数学证明，其逼近非线性变换后概率分布的精度优于UKF，并且能够解决UKF在处理高维非线性状态估计问题时滤波性能不佳甚至发散的问题。应当指出，UKF、二阶插值滤波和CKF在原理上都是基于一组加权样本点来逼近非线性状态的统计特性，且采样过程都是根据确定的数学表达式来实现的，因此，它们可以统一归类为确定采样型滤波器。本节主要以UKF算法为例展开论述。

EKF、UKF和PF算法的区别在于描述状态变量通过非线性系统后统计特性的方式不同，其中，EKF通过线性化技术实现对状态变量均值和方差的递推计算，UKF通过少量根据确定性方法选择的样本点描述随机变量通过非线性系统后的统计特性，而PF通过大量随机样本描述状态变量通过非线性系统后的分布，如图2-3所示。

常用滤波算法的性能特点如表2-3所示。

为了满足航天器自主导航研究的要求，需要从精度和计算量两个方面着眼，选择适当的非线性滤波算法。从精度方面考虑，采用UKF可以获得高于EKF的估计精度；从计算量方面考虑，EKF的计算量与状态向量维数的3次方成正比；UKF算法的计算量约为EKF算法的3倍；而PF的计算量与所选择的粒子数的大小有关，通常可达EKF的数百倍甚至上千倍，对于性能有限的星载计算机而言，应用PF算法计算负担较重。因此，建议集中研究EKF或UKF算法，即采用线性化或无迹变换技术，计算状态变量通过非线性系统后的均值和方差，在滤波过程中实现对状态变量的预测。此外，应用UKF算法不必求解雅克比矩阵，适用于地球重力场导航等雅克比矩阵不易求解的情况。UKF算法在2007年发射的美国海军NPSAT 1（Naval Postgraduate School Satellite 1）卫星上得到应用，该卫星的姿态确定系统利用UKF算法处理三轴磁强计的测量信息，获得卫星姿态及其变化率的估计值。

2.UKF算法流程

无迹卡尔曼滤波最先由Julier提出，又经Merve扩展出了多种算法。与EKF不同，UKF中采用无迹变换（Unscented Transform，UT）技术代替线性化，以描述状态变量通过非线性系统方程或测量方程后的均值和方差。所谓无迹变换，指的是通过确定性采样方法在状态空间选择若干样本（即Sigma点），用这些样本的分布来描述状态变量的均值和方差特性。可以证明，对每个Sigma点分别进行非线性变换，得到一组新的Sigma点，则这些新样本的均值和方差能够以较高精度逼近真实的状态变量经过非线性变换后的均值和方差。图2-4所示为二维状态变量的无迹变换示意图。

无迹变换的计算方法为：假设x的均值为，协方差为Pk，通过非线性函数x′=f（x）传播到x′，要计算x′的均值和协方差，需要构造一个由x的2n+1个采样点χi（称为Sigma点）组成的矩阵

式中，表示矩阵平方根的第i列；λ=α2（n+κ）-n，是一个标量。

常数α一般取小的正值（如10-4≤α≤1），控制西格玛点的分布状态。调节α可以减小非线性方程的高阶项影响。κ的具体取值虽然没有界限，但应确保矩阵（n+λ）Pk为半正定矩阵。对高斯分布的情况，当状态变量为单变量时，取κ=2；当状态变量为多变量时，取κ=3-n。

本节先给出UKF的算法流程，再说明该算法相对EKF的优势。

第1步：初始化，假定初始时刻未知状态的先验分布均值为，方差为P0。

第2步：选择Sigma点，给定上一步的状态估计值及其方差阵Pk-1，Sigma点可用下式选择

式中，可调参数a描述了Sigma点的散布，一般取为一个小正数；在一些特殊情况下，通过适当选择a的取值可以消除某些高次误差的影响。表示矩阵平方根的第i列，矩阵是通过矩阵分解得到的，满足Pk-1=。对于l维系统来说，需要选取2l+1个Sigma点。

第3步：预测，将各Sigma点分别代入状态转移函数进行计算，得到一组新的样本

状态变量的预测值和方差阵可按下式计算

式中，权值为

第4步：更新，根据预测的结果和Pk|k-1重新选取Sigma点，即

将重新选取的Sigma点依次代入测量方程进行计算，得到

观测量的预测均值及其方差Pzz可按下式计算

状态和观测量的互协方差阵Pxz为

通过观测量对预测值进行修正，得到状态估计值及其方差阵为

第2～4步反复迭代运行，就可以得到状态xk的估计值。

UKF与EKF的不同之处在于采用无迹变换技术代替线性化，计算状态变量通过非线性系统方程与测量方程后的均值和方差。可以证明，采用无迹变换技术可以精确描述非线性函数泰勒级数展开式的二次项，而EKF中所用的线性化技术仅考虑了泰勒级数展开式的一次项。相对EKF而言，UKF算法估计精度更高，并且不需要求解雅克比矩阵。

UKF算法的优势在于对预测分布均值的描述比EKF更准确。为了便于比较，先推导预测均值的表达式。将状态变量先验分布的均值和方差记为

则预测分布的均值为

式中，为估计误差。不难得到其统计特性为

将在处展成泰勒级数，得到

式中，ϕi∈ℝl的第i个元素为1，其他元素均为0; fi为f的第i项，从而

式（2-107）中给出了非线性函数泰勒级数展开式的二次项。

接下来计算UKF中通过无迹变换得到的样本均值，将式（2-98）代入式（2-99）第一式可得

将和在处展成泰勒级数，得到

整理上式得到

对比式（2-107）和式（2-108）可知，UKF中的样本均值与E{xk的泰勒级数展开式的一次项和二次项是相等的。由此可知，采用无迹变换技术得到的样本均值以二次精度逼近预测分布均值。同理，样本均值将以二次精度逼近预测均值。

回顾在EKF中对的近似方法，为了与UKF区别，用符号表示EKF的预测值

观察式（2-107）和式（2-109），不难发现，EKF简单地忽略了非线性函数泰勒级数展开式的高次项，给出的预测值二次项为0，与预测均值E{xkz1∶k-1}的二次项不符合。可见，EKF的对状态变量均值的预测误差体现在泰勒级数展开式的二次项和其他高次项中，而采用UKF得到的预测误差体现在泰勒级数展开式的二次以上项中。因此，UKF对预测均值的描述更精确，理论上能够达到比EKF更高的估计精度。

2.4.5 约束卡尔曼滤波

1．问题描述

在状态估计问题中，系统的状态往往满足一些代数约束，如果设计估计算法忽略这种约束，则估计的性能未必最优，而且最终估计未必满足约束。在本节中将对考虑等式约束的卡尔曼滤波进行研究。考虑两种典型的等式约束：线性等式约束（Linear Equality Constraint，LEC）和二次型等式约束（Quadratic Equality Constraint，QEC）。其中线性等式约束可以写作

式中，D∈ℝs×n; d∈ℝs; s≤n为约束的维数。

通常情况下，可以假设D行满秩。如果不是行满秩，则意味着存在冗余的约束，这时候可以去掉冗余的约束使得D行满秩。

对于二次型等式约束可以写作

式中，Ai=ATi∈ℝn×n; q为等式二次型约束的个数。

值得注意的是，对于一般的非线性等式约束模型有

其在参考点泰勒展开可以得到

式中，De为微分算子，有

式中，ej为e的第j个元素，e为扰动量。如果仅考虑一阶项，则约束可以写作线性等式约束的形式；如果考虑二阶项，则约束可以近似成二次型等式约束的形式。

对于最小均方误差估计问题，给出状态的先验估计和方差阵以及测量方程

式中，z∈ℝm; H∈ℝm×n行满秩为测量方程。求后验估计，使目标函数

极小，且满足约束方程式（2-112）。

2．状态线性等式约束滤波

状态线性等式约束滤波问题可以描述为：寻找最优估计∈ℝn使得目标函数式（2-116）最小，且满足式（2-110）给出的约束方程。

将写作先验估计和观测量z的线性组合形式，即

式中，K∈ℝn×m; N∈ℝn×n; n∈ℝn为待确定矩阵。在和η=0的情况下有=x，因此

从而使K、N和n满足约束

将式（2-119）代入式（2-117）中可以得到

式中，。

对于非约束线性估计问题，K的最优值可以通过对式（2-116）求极值得到。这里考虑的是线性等式约束估计，则极值函数可以重新写作

假设测量噪声和先验估计不相关，将式（2-120）代入式（2-121）中可以得到

式中，R为η的方差阵，P-为的方差阵。

利用最优必要条件可以得到

由式（2-123）第一式可以解得

其中

将式（2-124）代入式（2-123）第二式可得

经过简化可以解得

其中

将式（2-127）代入式（2-124）中可以得到约束估计的卡尔曼滤波增益矩阵，然后利用下式对方差进行测量更新

3．状态二次型等式约束滤波

状态二次型等式约束滤波问题可以描述为：寻找最优估计使得目标函数式（2-116）最小，且满足约束方程式（2-111），这里仅考虑单个约束的情况。和状态线性等式约束滤波类似，可以写成式（2-120）的形式。

相应的，二次型状态等式约束滤波的极值函数可以写作

式中，λ为拉格朗日乘子。假设测量噪声和先验估计不相关，将式（2-120）代入式（2-130）中可以得到

根据附录E的最优必要条件可以得到

上式第一式可以重新写作

式中，W由式（2-125）得到。

式（2-133）有K和λ两个未知参数需要求解。典型的求解方法就是从式（2-133）的第一式求解出K为λ的函数，然后代入式（2-133）的第二式中求解出λ。式（2-133）第一式为离散Sylvester方程。如果A=I，可以直接对矩阵求逆得到K的解。如果A为一般矩阵，K的解则没那么直观。下面进行具体分析。

对于m维空间，存在m-1个向量βi（i=1，2，…，m-1）使得βi⊥βj（i≠j）和ϵ⊥βi。因此，λϵϵTW-1的m个特征值为0，0，…，0，λε，其中ε由式（2-137）给出，相应的特征向量为Wβ1，Wβ2，…，Wβm-1，ϵ。假设存在m个αi∈ℝ，i=1，2，…，m，使得

因为W＞O，式（2-134）左乘以ϵTW-1得到α1=0，然后左乘以βTjW-1（j=1，2，…，m-1）得到

这意味着λϵϵTW-1的m个特征向量线性独立。换句话说，λϵϵTW-1可以对角化为

式中，（[γ 1，γ 2，…，γ m]）=diag（[0，0，…，0，λε]），V=[W β 1，Wβ2，…，Wβm-1，ϵ]。由于对称矩阵同样可以被正交矩阵对角化，因此A可以写作

式中，U为正交矩阵；Ξ=diag（[ξ1，ξ2，…，ξn]）。将式（2-137）代入式（2-133）的第一式，并左乘以UT、右乘以V可以得到

式中，K～=UTKV;C～=UTCV;C=（P-HT-λA^x-ϵT）W-1。记：K～和C～的第i、j个元素分别为k～ij和c～ij，式（2-138）可以写作

这意味着

一旦计算出来，则有

对于状态二次型等式约束滤波，很难得到后验估计的误差协方差阵。但是基于扩展卡尔曼滤波的假设，可以将其近似为式（2-129）。

注意到c～ij和γj依赖于λ，因此需要先计算λ才能计算K。对式（2-133）的第一式右乘以ϵ可以得到

式中，d=P-HT W-1ϵ。

则

将式（2-143）代入式（2-133）的第二式可以得到

对式（2-144）进一步化简可以得到

式中，h=d～+; d～=UTd; 。

式（2-145）可以写成标量的形式

式中，。通常情况下，可以采用牛顿迭代法对进行求解。但是这种方法只能求解出一个解，而实际上式（2-146）有2p个根，其中

式中，q为A的不同特征值个数。因此牛顿法得到的解未必是最优解，而且牛顿法未必能够保证收敛，这就需要更为有效的方法去计算。令为A的q个不同特征值，并定义多项式函数为

式（2-148）可以重新写作

式中，可以由差分方法计算得到。注意到式（2-148）和式（2-146）有相同的解。构造式（2-149）的伴随矩阵为

G的特征值对应式（2-149）的解，通过求解G的特征值可以求解出式（2-149）的所有解。因为G有2p个特征值，必须确定最优的那个特征值。利用约束优化的二阶充分条件可以得到如下定理：

定理2.6：如果为G的特征值，且对于∀i∈[1，r]，r=2，3，…n，有，则对应最优泛函指标。

证明：由附录E中的约束优化内容，可以将最优泛函式（2-131）可以重新写作

其中

假设K∗为式（2-142）的解，λ∗为式（2-148）的解。根据附录E约束优化的内容可知，如果g（K∗）=0，且对于所有满足下式

的z都有

则K∗对应最优泛函。

式（2-153）和式（2-154）中，vec为向量化算子；⊗为Kronecker算子（相关定义见附录C）。

为了判断式（2-154）的成立条件，构造矩阵

式中，Sr为对称分块矩阵；Lrr（K∗，λ∗）为L（K∗，λ∗）的左上角r ×r矩阵；（Im⊗▽gT）r矩阵为Im⊗▽gT的前r列。对于所有的z·▽g（K∗）=0，当且仅当下面条件成立时，有Q（z）＞0，即

因此问题的关键是判断式（2-157）的成立条件。为了推导方便，仅考虑标量测量数据的情形（m=1）。这样式（2-156）可以重新写作

Sr的行列式由下式得到

通常需要对上式进行计算得到det（Sr）的符号，但是det（Sr）＜0的一个充分条件为

这是由于det（I r+λε Ξ r）（I r+λε Ξ r）-1的第i个特征值为，从而得证。

注：

（1）状态二次型等式约束滤波的实施流程如图2-5所示。

（2）如果A=I，则二次型约束为范数约束。因为=1，q=1，则式（2-148）可以简化为

从而拉格朗日乘子的解可以写作

根据定理2.6可以得到上式中的“+”号对应极小泛函指标。

（3）定义Y∈ℝn ×n由下式给出

则变换x～Y=Yx ～可以将约束方程变换成

式中，的对角矩阵元素为0，1，-1。对于变量，其二次型系数矩阵最多有两个不同的非零特征值，这意味着拉格朗日乘子最多有4个根。

（4）对于约束系统，约束滤波得到的估计更为合理。比如自旋卫星的姿态估计问题，由于缺少陀螺测量量，而且状态变化很快，常用Markley变量替代姿态四元素表征姿态。Markley变量中的状态量包含角动量在惯性系和本体系下的投影Li和Lb，由于两者是同一个变量在不同坐标系下的投影，满足约束二次型等式约束‖Li‖=‖Lb‖。如果不考虑该约束，得到的估计未必合理。