[发明专利]一种常值机动空间目标的约束滤波追踪方法

权利要求书

1.一种常值机动空间目标的约束滤波追踪方法，包括如下步骤：

步骤一：建立目标问题模型；

在惯性系中，考虑一个对地三轴稳定并推力大小恒定的航天器在轨道上运动，其运动可用离散时间状态空间方程表示

x_k+1＝A_kx_k+G_kd_k+Γ_kw_k (1.a)

y_k＝C_kx_k+v_k (1.b)

其中x_k∈Rⁿ表示系统状态，d_k∈R^m表示常值机动矢量；由于航天器相对惯性坐标系做姿态运动，因此机动矢量的方向是变化的，可以描述为

d_k+1＝(I+δ_k)d_k (2)

其中δ_k∈R^m×m是一个未知的时变矩阵，因为航天器的姿态变化缓慢，于是有

δ_k≈0 (3)

但机动矢量满足范数约束

||d_k||＝ρ其中ρ＞0 (4)

并且y_k∈R^p表示测量矢量，过程噪声w_k∈R^m和测量噪声v_k∈R^m都是白噪声，其协方差矩阵分别为Q_k∈R^m×m，R_k∈R^p×p，并且Q_k＞0，R_k＞0；矩阵A_k，G_k，Γ_k和C_k具有合适的维度

rank[G_k]＝rank[Γ_k]＝m (5)

并且(A_k，C_k)是能观测的，(A_k，G_k)和(A_k，Γ_k)是可控的；

步骤二：建立无约束扩维卡尔曼滤波器；

A.扩维系统

扩维卡尔曼滤波(ASKF)将未知机动作为状态的一部分，扩维后状态量表示为

其中X_k∈R^n+m表示扩维状态矢量，因此，系统(1)可表示为

其中上标横杠表示扩维符号，有

于是和可以表示为

进而可以得到

B.建立无约束扩维卡尔曼滤波(UASKF)

假设初始机动是高斯随机变量，无约束估计由下式确定

互协方差为

基于扩维系统(7)，假设机动时不变的并且令δ_k＝0，那么无约束扩维卡尔曼滤波器可以表示为

其中，是一步预测，是先验状态协方差，是后验协方差，是卡尔曼增益矩阵，η_k+1表示残差矢量

注意到如果δ_k＝0，那么无约束估计器(10)能够稳定工作并且产生最优结果；

无约束的先验和后验估计误差定义为

将式(7)和式(10.a)代入式(11)中，可以得出

同样的，将式(10.e)和式(10.f)代入到式(12)中，后验估计误差可以表示为

进一步，将式(13)代入式(14)并得到

将后验估计误差和增益矩阵分解为

根据式(16)，式(15)可以重写为

考虑到式(17)和式(18)，于是有

当时间趋于无穷时k→∞于是我们有

显然，状态估计是有偏的，这主要是由于机动的变化引起的；

步骤三：在无约束扩维卡尔曼滤波器中施加局部约束；

施加机动范数约束，减少因式(3)近似而带来的负面影响，重建估计器(10)；将扩维后验协方差矩阵定义为

其中局部协方差定义为

另外，考虑满足式(4)中约束的归一化机动估计

将式(10.e)分解为

于是有

将式(28)代入式(26)中，有

因此，考虑到范数约束，可以通过最小化扩维目标函数来获得最优估计：

式(30)的右侧满足

其中tr{ }表示矩阵的迹；为方便起见，将局部性能指数定义为

其中标量λ_k+1表示拉格朗日乘数；显然，由于和分别仅取决于和因此，J_k+1的最小化可以分别对和进行最小化，于是有

A.对系统状态施加局部约束后的估计

通过最小化基于卡尔曼滤波器示例构造局部估计器并不困难；省去赘述，我们直接表示估算如下：

预测部分为

修正部分如下

其中Θ_k+1为

B.对机动施加局部约束后的估计

对的最小化

通过最小化性能指标来构造机动的局部估计；基于估计器(10)

考虑到

对迹的最小化等于对协方差的最小化，然后将式(38)代入式(33)中，对式(33)取和λ_k+1的偏导数，然后将其等于零，于是最小化的一阶条件可以表示为

展开式(39)和式(40)

从式(41)，由于det(Θ+λ_k+1η_k+1η_k+1^T)≠0(在式(58)中证明)，其符合

而对于等式的后半部分

把式(44)代入(43)得到

此外，将式(45)代入(42)中，得到关于λ_k+1的二阶方程

方程中b和c的值可以表示为

通过对式(46)使用韦达定理

其中可以表示成

式(49)表明对于最优拉格朗日乘数有两种可能的解，为了确定最小化的符号，应该检查二阶条件；

在式(49)中，符号为正时性能指数将最小化，而在选择负号时最大化；当增益是列矢量时，可以通过再次对式(39)进行微分来获得性能指标的海森矩阵；

式(51)显示，海森矩阵是对角矩阵，根据式(49)

根据式(50)，式(52)可以改写为

回顾式(35)，可以得出

其中r是标量测量噪声的协方差，更进一步，根据式(53)和式(54)，当选择正号时，海森矩阵是正定的，并且出现最小性能指数；相反的，就会出现最大性能指标；

为了证明在式(49)中取正号时，矩阵是正定的，把项改写为

等号两边同时乘以η_k+1有

对式(50)，在式两边同时乘以有

因为Θ_k+1是正定的，于是有最终可以从式(56)中得到

式(58)表明矩阵是正定的，于是

C.协方差矩阵的更新

最小性能指标的最优拉格朗日乘数可以表示为

然后将式(59)代入式(45)中得

其中是与扰动相关的局部卡尔曼增益矩阵，并且

根据卡尔曼滤波算法，约束机动估计可以通过将一步预测校正为如下形式

其中是无约束估计，显然，约束估计被归一化以满足式(26)中的约束，并且，局部约束卡尔曼增益将估计值投影到由约束条件跨越的m维欧几里德表面，而不是无约束条件方向；

除此之外，还有

其中可以表示为

将式(65)代入式(64)中有

因为测量噪声总是与和不相关，因此，式(66)可以通过略去与和相关项来重新表述

最终，可以得出

2.根据权利要求1所述的一种常值机动空间目标的约束滤波追踪方法，其特征在于：如步骤三所示，在扩维卡尔曼滤波器的基础上，对机动施加范数约束，从而减少因δ_k≈0近似而带来的负面影响，于是可以重新构建估计器，进而完成算法的更新和迭代。