CN107702709A

CN107702709A - 一种非合作目标运动与惯性参数的时频域混合辨识方法

Info

Publication number: CN107702709A
Application number: CN201710772043.5A
Authority: CN
Inventors: 黄攀峰; 韩冬; 刘正雄; 孟中杰; 张夷斋; 张帆
Original assignee: Northwestern Polytechnical University
Current assignee: Northwestern Polytechnical University
Priority date: 2017-08-31
Filing date: 2017-08-31
Publication date: 2018-02-16
Anticipated expiration: 2037-08-31
Also published as: CN107702709B

Abstract

本发明涉及一种非合作目标运动与惯性参数的时频域混合辨识方法，首先通过视觉相机获得目标的位姿信息。本文提出一种在视觉测量目标位姿的基础上，解耦运动过程中的平动与转动，时域与频域同步辨识目标的状态与动力学参数，频域上采用DFT辨识与平动相关参量，时域上线性化空间漂浮目标动力学方程，采用EFIR辨识与转动相关参量。

Description

一种非合作目标运动与惯性参数的时频域混合辨识方法

技术领域

本发明属于航天器参数辨识领域，涉及一种非合作目标运动与惯性参数的时频域混合辨识方法。

背景技术

空间操作作为未来航天技术发展的方向，越来越受到航天大国的重视。由平台基座、操作机械臂组成的空间机械臂机器人可广泛应用于在轨加注、故障卫星维护、轨道垃圾清理、辅助变轨等在轨服务任务。空间机械臂机器人为空间机器人的一种，其具有较大的工作空间和灵巧的操作性能。已经成为当前空间操作的主要手段。

空间操作的前提是对目标的抓捕，由于被抓捕对象为非合作目标，其运动状态与惯性参数未知，这对于抓捕及抓捕后复合体的稳定控制造成了很大的影响，已有的研究成果及实验都是针对参数已知的合作目标，或者采用激励的方法进行辨识，需要与目标物理连接，这在目标完全未知的条件下是有风险的。而已有的基于视觉的参数辨识都是在时域进行处理，需要提前已知或假设量测噪声与过程噪声，这在空间复杂背景条件下又是极其苛刻的。并且由于待估计参数较多，需要更长的处理时间，这对辨识精度与燃料消耗都是极其不利的。因此有必要发明一种更准确更快速的非合作目标参数辨识方法，以获得目标的运动速度、角速度以及惯性参量等参数。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种非合作目标运动与惯性参数的时频域混合辨识方法，主要针对具备视觉观测系统的空间智能机器人对非合作目标的目标位置、平动速度、转动角速度以及惯性参量等运动参量及惯性参量的辨识方法.

技术方案

一种非合作目标运动与惯性参数的时频域混合辨识方法，其特征在于步骤如下：

步骤1：由视觉相机获得的非合作目标相对位姿信息；

步骤2、根据视觉相机获得的目标位姿信息以及航天器运动方程，采用频域最优滤波器对非合作目标的质心位置及平动速度进行估计：

2-1)设计频域滤波器，对非合作目标位置序列进行DFT变换，相当于把这个序列通过一个FIR数字滤波器，位置序列r_s(n)的离散付里叶变换式

式中，N为变换点数目，r_s(n)为n时刻目标的位置，由视觉相机获得，如图1所示。这相当于使序列r_s(n)中的每个样本有相位滞后最后在n＝N-1时刻把经过相移的各个样本叠加起来，就得出了序列r_s(n)中频率为kΩ的频谱分量的付里叶系数X(k)；

2-2)由于目标存在平动与转动，将方程进行平动与转动分解，

r_s＝r+R(q)ρ＝r₀+vt+R(q)ρ

其中：ρ为卫星质心到目标点位置矢量，ω为目标旋转角速度，ρ,ω均为在相机所在坐标系下的表达，r为目标质心位置，r₀为初始时刻目标质心位置；相机所在坐标系相对于惯性坐标系的旋转通过四元数q表示；

角速度与四元数的运动学关系表示为：

式中，同样，对于姿态变化所采用的四元数乘法，做如下定义：

式中，E为单位矢量，q_v为四元数矢量部分，q₀为四元数标量部分，四元数表示为

四元数运算通过旋转矩阵R(q₂)R(q₁)描述，R(q)定义为：

质心位置r₀与平动线速度v均为目标位置序列及其差分的直流分量；

步骤3、辨识质心位置r₀与平动线速度v：根据视觉相机获得的目标位姿信息以及航天器运动方程，在时域上采用扩展有限冲击相应滤波器对非合作目标的运动角速度以及惯性参量进行估计：

3-1)设计时域滤波器，

EFIR滤波器首先假定窗口长度N，通过计算量测偏差的均方值在线估计得出；当获得第n时刻量测值时，从时刻m＝n-N+1到时刻n之间的N个量测值为有效量测值，目标状态的估计值表示为

其中，为l时刻状态估计值，为状态预估计，l取值范围从m+k到n，k为状态数，当l＝n时，输出n时刻状态估计值。Z_l为l时刻观测值，K_l为l时刻滤波器增益，为观测矩阵；

式中G_l为广义噪声功率增益，通过下式迭代计算得出

对与转动有关的状态矢量的辨识流程如下所示：

输出即为目标参数的状态估计值；

其中：

所述求解H和Φ的步骤如下：

根据航天器动力学方程，设计需要估计的状态变量x、推导算法所需要的观测矩阵H以及状态转移矩阵Φ表达式：

建立通过欧拉方程描述的目标姿态动力学方程：

其中，I为转动惯量，τ为扰动力矩；

引入惯量参数表示：

式中，I_xx,I_yy,I_zz为目标主惯量，满足以下条件：

I_xx+I_yy＞I_zz

I_yy+I_zz＞I_xx

I_zz+I_xx＞I_yy

进而求得惯量参数的约束条件

p_x＞-1,p_y＞-1,p_z＞-1

用惯性参数改写动力学方程为：

式中

I_c＝diag(I_xx,I_yy,I_zz)，tr(I_c)表示I_c的迹，ε_τ用来描述太阳能帆板支架、重力梯度产生的阻尼效果，选择采用过程噪声来描述；

视觉测量系统获得带有噪声的目标点相对位姿并解算到惯性坐标系下，目标位置由矢量r_s表示，目标所在目标坐标系{C}相对于惯性坐标系{A}的姿态由四元数η表示，测量向量Z表示为：

式中：为测量噪声，对于空间自旋目标，测量得到的目标姿态η由预先未知的两个姿态变换得到，设目标坐标系{C}相对于相机坐标系{B}的姿态由四元数μ表示；相机坐标系{B}相对于惯性坐标系{A}的姿态由四元数q表示，则有下式成立：

式中为四元数乘，定义与转动有关的状态矢量：

x＝[q_v ^T ω^T p^T ρ^T μ_v ^T]^T

式中：q_v,μ_v分别为四元数的矢量部分；则观测方程中转动部分表示为：

式中

该方程为四元数q,μ的非线性方程，为满足线性滤波器的要求，需要对观测方程线性化；有||δq_v||＜＜1，||δμ_v||＜＜1,δq₀≈1，观测矩阵中位置部分的线性化由公式得一阶近似公式：

描述微小旋转的四元数方程：

将公式代入公式并忽略高阶小项得：

式中：

至此，

公式给出了观测方程的一阶近似值，并由此得出量测矩阵：

根据公式所描述的目标动力学方程，将连续时间状态空间模型描述为：

式中，为过程噪声，考虑到自适应EFIR滤波器为线性滤波器，为满足滤波器要求，

对状态方程线性化为：

对状态方程线性化直接由全微分公式给出：

根据刚体转动的性质，设状态参数中微分不变量为θ，则并且由此可得出连续时间状态转移矩阵：

将方程写成离散形式，有

X(n+1)＝Φ(n)X(n)+V(n)

其中，V(n)表示离散的过程噪声，Φ(n)表示离散状态转移矩阵；

对于时常系统有

有益效果

本发明提出的一种非合作目标运动与惯性参数的时频域混合辨识方法，包括以下步骤：1)由视觉相机获得的非合作目标相对位姿信息；2)根据视觉相机获得的目标位姿信息以及航天器运动方程，采用频域最优滤波器对非合作目标的质心位置及平动速度进行估计；3)根据视觉相机获得的目标位姿信息以及航天器运动方程，在时域上采用扩展有限冲击相应滤波器对非合作目标的运动角速度以及惯性参量进行估计。

与现有技术相比，本发明具有如下有益效果：

本发明的基本思想是将航天器运动分解为平动与转动，采用数据的时域与频域同步处理，针对每一时刻检测获得的数据，采用扩展有限冲击响应滤波器对参数进行估计，获得目标的转动参数。对于成块数据，采用离散傅立叶变换加以处理，从而获得目标的平动参数。时频域的同步处理最大限度的使用了采样数据所提供的信息，降低了滤波器的估计维度，提高了处理速度，缩短了估计时间。加窗滤波器的引入提高了估计算法的鲁棒性，增加了对噪声的抑制能力。本发明具体拥有以下优点：

1.估计速度快

本发明将航天器运动分解为平动与转动，采用数据的时域与频域同步处理，针对每一时刻检测获得的数据，采用扩展有限冲击响应滤波器对参数进行估计，获得目标的转动参数。对于成块数据，采用离散傅立叶变换加以处理，从而获得目标的平动参数。时频域的同步处理最大限度的使用了采样数据所提供的信息，降低了滤波器的估计维度，提高了处理速度，缩短了估计时间。

2.鲁棒性强

本发明针对对传统的卡尔曼滤波器中对测量噪声与过程噪声估计不准确所造成的滤波发散问题，采用滤波器加窗处理，增强了系统对噪声的抑制能力，提高了系统的鲁棒性。

3.实时性高

本发明采用时频域同时处理的方法，减少了时域滤波器的状态维度，减小了计算量，从而缩短了运算时间，提高了系统的实时性。

附图说明

图1为目标位置测量示意图

图2为质心位置与线速度频率域估计

图3为目标位置角速度及惯性参数时域估计

具体实施方式

现结合实施例、附图对本发明作进一步描述：

为实现上述目的，本发明所采用的技术方案包括以下步骤：

1)由视觉相机获得的非合作目标相对位姿信息

2)根据视觉相机获得的目标位姿信息以及航天器运动方程，采用频域最优滤波器对非合作目标的质心位置及平动速度进行估计。

3)根据视觉相机获得的目标位姿信息以及航天器运动方程，在时域上采用扩展有限冲击相应滤波器对非合作目标的运动角速度以及惯性参量进行估计。

所述步骤1)中，计算目标惯性系坐标的具体步骤如下：

1-1)：设相机坐标系中目标位置为P(x_c,y_c,z_c),目标在相机坐标系中成像位置(u,v)，则有以下公式成立：

式中(u₀,v₀)为图像坐标系的中心，(a_x,a_y)为有效焦距长度。

1-2)：待求惯性系下坐标P(x_w,y_w,z_w)便可以表示为：

其中旋转矩阵R、平移矩阵T分别为：

T＝[t_x,t_y,t_z]^T (3)

1-3)：将公式(3)，(1)代入(2)，可得到

其中B＝[x_w,y_w,z_w,1]^T，待求惯性系下的坐标P(x_w,y_w,z_w)可通过对相机坐标系中成像位置(u,v)的实时检测及公式(4)解算获得，坐标点的集合便构成目标运动轨迹。

所述步骤2)中，根据视觉相机获得的目标位姿信息以及航天器运动方程，采用频域最优滤波器对非合作目标的质心位置及平动速度进行估计的具体方法如下：

2-1)设计频域滤波器，对目标位置序列进行DFT变换，相当于把这个序列通过一个FIR数字滤波器，位置序列r_s(n)的离散付里叶变换式

式中，N为变换点数目，r_s(n)为n时刻目标的位置，由视觉相机获得，如图1所示。这相当于使序列r_s(n)中的每个样本有相位滞后最后在n＝N-1时刻把经过相移的各个样本叠加起来，就得出了序列r_s(n)中频率为kΩ的频谱分量的付里叶系数X(k)。

2-2)推导目标位置方程，由于目标存在平动与转动，将方程进行平动与转动分解。

r_s＝r+R(q)ρ＝r₀+vt+R(q)ρ (6)

如图1所示，ρ为卫星质心到目标点位置矢量，ω为目标旋转角速度，ρ,ω均为在坐标系{B}下的表达，r为目标质心位置，r₀为初始时刻目标质心位置。坐标系{B}相对于{A}的旋转通过四元数q表示，角速度与四元数的运动学关系可表示为：

式中，同样，对于姿态变化所采用的四元数乘法，也可做如下定义：

式中，E为单位矢量，q_v为四元数矢量部分，q₀为四元数标量部分，四元数表示为四元数运算可通过旋转矩阵R(q₂)R(q₁)描述，R(q)定义为：

根据公式(6)、(7)，质心位置r₀与平动线速度v均为目标位置序列及其差分的直流分量，因此可通过DFT滤波器准确估计出，并且由于估计值为直流分量，因而对高频观测噪声以及信号的非等间隔采样具有很强抑制性，算法鲁棒性好。估计结果入图2所示。

所述步骤3)中，根据视觉相机获得的目标位姿信息以及航天器运动方程，在时域上采用扩展有限冲击相应滤波器对非合作目标的运动角速度以及惯性参量进行估计的具体方法如下：

3-1)设计时域滤波器，由于空间环境复杂的特点，传统卡尔曼滤波器所要求的提前预知量测噪声与过程噪声无法满足，在噪声估计不准确的情况下，卡尔曼滤波器会产生误差累计进而发散，故本发明采用加窗处理的扩展有限冲击响应(EFIR)滤波器，本发明将其首次应用于航天器惯性参数及角速度估计。不同于采用递归算法的卡尔曼滤波器，EFIR滤波器采用迭代算法，因为在算法中做了加窗处理，不会产生因持续的偏差累计而导致滤波器发散的现象，鲁棒性好。

EFIR滤波器首先假定窗口长度N，通过计算量测偏差的均方值在线估计得出。当获得第n时刻量测值时，从时刻m＝n-N+1到时刻n之间的N个量测值为有效量测值，这使得滤波器不需要预先知道噪声特性参数。目标状态的估计值表示为

其中，为l时刻状态估计值，为状态预估计，l取值范围从m+k到n，k为状态数，当l＝n时，输出n时刻状态估计值。Z_l为l时刻观测值，K_l为l时刻滤波器增益，为观测矩阵。

式中G_l为广义噪声功率增益，通过下式迭代计算得出

可见，G_l的计算仅需已知矩阵H_l与Φ_l，H_l为l时刻观测矩阵，Φ_l为l时刻状态转移矩阵，在滤波器增益计算中，不需要预测噪声统计特性，该特点使算法适合噪声特性未知的复杂条件下的空间任务。算法流程如下所示。

输出即为目标参数的状态估计值，估计结果如图3所示

3-2)根据航天器动力学方程，设计需要估计的状态变量x、推导算法所需要的观测矩阵H以及状态转移矩阵Φ表达式

为了估计目标的运动状态与参数，建立通过欧拉方程描述的目标姿态动力学方程：

其中，I为转动惯量，τ为扰动力矩，考虑到绕坐标轴三个方向转动的耦合性，无法通过目标运动轨迹分别独立估计出三个方向转动惯量，为解决这个问题，引入惯量参数表示：

式中，I_xx,I_yy,I_zz为目标主惯量，应满足以下条件：

I_xx+I_yy＞I_zz

I_yy+I_zz＞I_xx

I_zz+I_xx＞I_yy

进而可求得惯量参数的约束条件

p_x＞-1,p_y＞-1,p_z＞-1

用惯性参数改写动力学方程为：

式中

I_c＝diag(I_xx,I_yy,I_zz)，tr(I_c)表示I_c的迹，ε_τ用来描述太阳能帆板支架、重力梯度等产生的阻尼效果，可以选择采用过程噪声来描述。

假定视觉测量系统可以获得带有噪声的目标点相对位姿并可解算到惯性坐标系下，如图1所示，目标位置由矢量r_s表示，目标所在坐标系{C}相对于惯性坐标系{A}的姿态由四元数η表示，测量向量Z可表示为：

式中：为测量噪声，对于空间自旋目标，测量得到的目标姿态η由预先未知的两个姿态变换得到，设坐标系{C}相对于坐标系{B}的姿态由四元数μ表示。坐标系{B}相对于{A}的姿态由四元数q表示，则有下式成立：

式中为四元数乘，定义与转动有关的状态矢量：

x＝[q_v ^T ω^T p^T ρ^T μ_v ^T]^T (19)

式中：q_v,μ_v分别为四元数的矢量部分。则观测方程中转动部分可表示为：

式中

该方程为四元数q,μ的非线性方程，为满足线性滤波器的要求，需要对观测方程线性化。考虑到在采样时间较短的条件下，目标有很小的位姿变化，从而有||δq_v||＜＜1，||δμ_v||＜＜1,δq₀≈1，观测矩阵中位置部分的线性化可由公式(10)得一阶近似公式：

描述微小旋转的四元数方程：

将公式(22)代入公式(18)并忽略高阶小项可得：

式中：

至此，式(22)、(24)给出了观测方程的一阶近似值，并可以由此得出量测矩阵：

根据公式(8)、(16)所描述的目标动力学方程，将连续时间状态空间模型描述为：

式中，为过程噪声，考虑到自适应EFIR滤波器为线性滤波器，为满足滤波器要求，需要对状态方程(8)、(16)进行线性化。公式(8)可线性化表示为：

对于方程(16)的线性化可直接由全微分公式给出：

将方程(26)写成离散形式，有

X(n+1)＝Φ(n)X(n)+V(n) (28)

其中，V(n)表示离散的过程噪声，Φ(n)表示离散状态转移矩阵。对于时常系统有

本方法用于空间在轨服务任务中，具有不需要提前已知量测噪声与过程噪声，时域与频域同步处理的特点。本发明具有鲁棒性强，辨识速度快，辨识精度高等特点。

Claims

1.一种非合作目标运动与惯性参数的时频域混合辨识方法，其特征在于步骤如下：

步骤1：由视觉相机获得的非合作目标相对位姿信息；

<mrow> <mi>X</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <munderover> <mo>&Sigma;</mo> <mrow> <mi>n</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mi>N</mi> <mo>-</mo> <mn>1</mn> </mrow> </munderover> <msub> <mi>r</mi> <mi>s</mi> </msub> <mrow> <mo>(</mo> <mi>n</mi> <mo>)</mo> </mrow> <msup> <mi>e</mi> <mrow> <mo>-</mo> <mi>j</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mn>2</mn> <mi>&pi;</mi> </mrow> <mi>N</mi> </mfrac> <mo>)</mo> </mrow> <mi>k</mi> <mi>n</mi> </mrow> </msup> <mo>,</mo> <mrow> <mo>(</mo> <mi>k</mi> <mo>=</mo> <mn>0</mn> <mo>,</mo> <mn>1</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <mi>N</mi> <mo>-</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>

2-2)由于目标存在平动与转动，将方程进行平动与转动分解，

r_s＝r+R(q)ρ＝r₀+vt+R(q)ρ

<mrow> <msub> <mover> <mi>r</mi> <mo>&CenterDot;</mo> </mover> <mi>s</mi> </msub> <mo>=</mo> <mi>v</mi> <mo>+</mo> <mi>R</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>&omega;</mi> <mo>&times;</mo> <mi>&rho;</mi> <mo>)</mo> </mrow> </mrow>

角速度与四元数的运动学关系表示为：

<mrow> <mover> <mi>q</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mi>&Omega;</mi> <mrow> <mo>(</mo> <mi>&omega;</mi> <mo>)</mo> </mrow> <mi>q</mi> </mrow>

<mrow> <mi>q</mi> <mo>&CircleTimes;</mo> <mover> <mo>=</mo> <mi>&Delta;</mi> </mover> <msub> <mi>q</mi> <mn>0</mn> </msub> <msub> <mi>E</mi> <mn>4</mn> </msub> <mo>+</mo> <mi>&Omega;</mi> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mi>v</mi> </msub> <mo>)</mo> </mrow> </mrow>

式中，E为单位矢量，q_v为四元数矢量部分，q₀为四元数标量部分，四元数表示为q＝[q_v ^Tq₀]^T；

四元数运算通过旋转矩阵R(q₂)R(q₁)描述，R(q)定义为：

R(q)＝(2q₀ ²-1)E₃+2q₀[q_v×]+2q_vq_v ^T

3-1)设计时域滤波器，

式中G_l为广义噪声功率增益，通过下式迭代计算得出

对与转动有关的状态矢量x＝[q_v ^T ω^T p^T ρ^T μ_v ^T]^T的辨识流程如下所示：

输出即为目标参数的状态估计值；

其中：

<mrow> <mtable> <mtr> <mtd> <mrow> <mi>&Phi;</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>)</mo> </mrow> <mover> <mo>=</mo> <mi>&Delta;</mi> </mover> <mi>&Phi;</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>n</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>,</mo> <msub> <mi>t</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> <mi>&Phi;</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>n</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>t</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <msup> <mi>e</mi> <mrow> <mi>F</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>n</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>t</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> </mrow> </msup> <mo>&ap;</mo> <msub> <mi>E</mi> <mn>15</mn> </msub> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mrow> <mi>n</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>t</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> <mi>F</mi> <mrow> <mo>(</mo> <msub> <mi>t</mi> <mi>n</mi> </msub> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>.</mo> </mrow>

2.根据权利要求1所述非合作目标运动与惯性参数的时频域混合辨识方法，其特征在于：所述求解H和Φ的过程如下：

建立通过欧拉方程描述的目标姿态动力学方程：

<mrow> <mi>I</mi> <mover> <mi>&omega;</mi> <mo>&CenterDot;</mo> </mover> <mo>+</mo> <mi>&omega;</mi> <mo>&times;</mo> <mi>I</mi> <mi>&omega;</mi> <mo>=</mo> <mi>&tau;</mi> </mrow>

其中，I为转动惯量，τ为扰动力矩；

引入惯量参数表示：

式中，I_xx,I_yy,I_zz为目标主惯量，满足以下条件：

I_xx+I_yy＞I_zz

I_yy+I_zz＞I_xx

I_zz+I_xx＞I_yy

进而求得惯量参数的约束条件

p_x＞-1,p_y＞-1,p_z＞-1

用惯性参数改写动力学方程为：

<mrow> <mover> <mi>&omega;</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mi>d</mi> <mrow> <mo>(</mo> <mi>&omega;</mi> <mo>,</mo> <mi>p</mi> <mo>)</mo> </mrow> <mo>+</mo> <mi>B</mi> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> <msub> <mi>&epsiv;</mi> <mi>&tau;</mi> </msub> </mrow>

式中

<mrow> <mi>d</mi> <mrow> <mo>(</mo> <mi>&omega;</mi> <mo>,</mo> <mi>p</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>p</mi> <mi>x</mi> </msub> <msub> <mi>&omega;</mi> <mi>y</mi> </msub> <msub> <mi>&omega;</mi> <mi>z</mi> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>p</mi> <mi>y</mi> </msub> <msub> <mi>&omega;</mi> <mi>x</mi> </msub> <msub> <mi>&omega;</mi> <mi>z</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>p</mi> <mi>z</mi> </msub> <msub> <mi>&omega;</mi> <mi>x</mi> </msub> <msub> <mi>&omega;</mi> <mi>y</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <mi>B</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>b</mi> <mi>x</mi> </msub> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>b</mi> <mi>y</mi> </msub> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>b</mi> <mi>z</mi> </msub> <mrow> <mo>(</mo> <mi>p</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

<mrow> <msub> <mi>&epsiv;</mi> <mi>&tau;</mi> </msub> <mo>=</mo> <mfrac> <mi>&tau;</mi> <mrow> <mi>t</mi> <mi>r</mi> <mrow> <mo>(</mo> <msub> <mi>I</mi> <mi>c</mi> </msub> <mo>)</mo> </mrow> </mrow> </mfrac> </mrow>

<mrow> <mi>&eta;</mi> <mo>=</mo> <mi>&mu;</mi> <mo>&CircleTimes;</mo> <mi>q</mi> </mrow>

式中为四元数乘，定义与转动有关的状态矢量：

x＝[q_v ^T ω^T p^T ρ^T μ_v ^T]^T

式中

<mrow> <mi>h</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>R</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mi>&rho;</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&mu;</mi> <mo>&CircleTimes;</mo> <mi>q</mi> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

该方程为四元数q,μ的非线性方程，为满足线性滤波器的要求，需要对观测方程线性化；有||δq_v||＜＜1，||δμ_v||＜＜1,δq₀≈1，观测矩阵中位置部分的线性化由公式R(q)＝(2q₀ ²-1)E₃+2q₀[q_v×]+2q_vq_v ^T得一阶近似公式：

<mrow> <msub> <mi>r</mi> <mi>s</mi> </msub> <mo>-</mo> <mi>r</mi> <mo>=</mo> <mi>R</mi> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>E</mi> <mn>3</mn> </msub> <mo>+</mo> <mn>2</mn> <mo>&lsqb;</mo> <msub> <mi>&delta;q</mi> <mi>v</mi> </msub> <mo>&times;</mo> <mo>&rsqb;</mo> <mo>)</mo> </mrow> <mi>&rho;</mi> </mrow>

描述微小旋转的四元数方程：

<mrow> <mi>&delta;</mi> <mi>q</mi> <mo>=</mo> <mi>q</mi> <mo>&CircleTimes;</mo> <msup> <mover> <mi>q</mi> <mo>^</mo> </mover> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow>

<mrow> <mi>&delta;</mi> <mi>&mu;</mi> <mo>=</mo> <msup> <mover> <mi>&mu;</mi> <mo>^</mo> </mover> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> <mo>&CircleTimes;</mo> <mi>&mu;</mi> </mrow>

将公式代入公式并忽略高阶小项得：

<mrow> <mi>&eta;</mi> <mo>=</mo> <mover> <mi>&eta;</mi> <mo>^</mo> </mover> <mo>+</mo> <mi>&Gamma;</mi> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>,</mo> <mover> <mi>&mu;</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <mo>&lsqb;</mo> <msub> <mi>&delta;q</mi> <mi>v</mi> </msub> <mo>+</mo> <msub> <mi>&delta;&mu;</mi> <mi>v</mi> </msub> <mo>&rsqb;</mo> </mrow>

式中：

<mrow> <mi>&Gamma;</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <msub> <mover> <mi>&eta;</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <msub> <mi>E</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>&lsqb;</mo> <msub> <mover> <mi>&mu;</mi> <mo>^</mo> </mover> <mi>v</mi> </msub> <mo>&times;</mo> <mo>&rsqb;</mo> <mo>+</mo> <msub> <mover> <mi>&eta;</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>&lsqb;</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>v</mi> </msub> <mo>&times;</mo> <mo>&rsqb;</mo> <mo>-</mo> <mo>&lsqb;</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>v</mi> </msub> <mo>&times;</mo> <mo>&rsqb;</mo> <mo>&lsqb;</mo> <msub> <mover> <mi>&mu;</mi> <mo>^</mo> </mover> <mi>v</mi> </msub> <mo>&times;</mo> <mo>&rsqb;</mo> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>v</mi> </msub> <msup> <msub> <mover> <mi>&mu;</mi> <mo>^</mo> </mover> <mi>v</mi> </msub> <mi>T</mi> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <mrow> <mo>(</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>v</mi> </msub> <mo>&times;</mo> <msub> <mover> <mi>&mu;</mi> <mo>^</mo> </mover> <mi>v</mi> </msub> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>-</mo> <msub> <mover> <mi>&eta;</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <msup> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mi>v</mi> </msub> <mi>T</mi> </msup> <mo>-</mo> <msub> <mover> <mi>q</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <msup> <msub> <mover> <mi>&mu;</mi> <mo>^</mo> </mover> <mi>v</mi> </msub> <mi>T</mi> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

至此，

公式给出了观测方程的一阶近似值，并由此得出量测矩阵：

<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>H</mi> <mo>=</mo> <mfrac> <mrow> <mo>&part;</mo> <mi>h</mi> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&part;</mo> <mi>x</mi> </mrow> </mfrac> <msub> <mo>|</mo> <mrow> <mi>x</mi> <mo>=</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>-</mo> <mn>2</mn> <mi>R</mi> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <mo>&lsqb;</mo> <mover> <mi>&rho;</mi> <mo>^</mo> </mover> <mo>&times;</mo> <mo>&rsqb;</mo> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>6</mn> </mrow> </msub> </mtd> <mtd> <mrow> <mi>R</mi> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mn>3</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>&Gamma;</mi> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>,</mo> <mover> <mi>&mu;</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>4</mn> <mo>&times;</mo> <mn>6</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>4</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <mrow> <mi>&Gamma;</mi> <mrow> <mo>(</mo> <mover> <mi>q</mi> <mo>^</mo> </mover> <mo>,</mo> <mover> <mi>&mu;</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> </mfenced>

<mrow> <mover> <mi>x</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <msub> <mi>f</mi> <mi>s</mi> </msub> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> <mo>+</mo> <mover> <mi>v</mi> <mo>~</mo> </mover> </mrow>

对状态方程线性化为：

<mrow> <mfrac> <mi>d</mi> <mrow> <mi>d</mi> <mi>t</mi> </mrow> </mfrac> <msub> <mi>&delta;q</mi> <mi>v</mi> </msub> <mo>=</mo> <mo>-</mo> <mi>&omega;</mi> <mo>&times;</mo> <msub> <mi>&delta;q</mi> <mi>v</mi> </msub> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mi>&delta;</mi> <mi>&omega;</mi> </mrow>

<mrow> <mfrac> <mi>d</mi> <mrow> <mi>d</mi> <mi>t</mi> </mrow> </mfrac> <msub> <mi>&delta;q</mi> <mn>0</mn> </msub> <mo>=</mo> <mn>0</mn> </mrow>

对状态方程线性化直接由全微分公式给出：

<mrow> <mfrac> <mrow> <mo>&part;</mo> <mi>d</mi> </mrow> <mrow> <mo>&part;</mo> <mi>&omega;</mi> </mrow> </mfrac> <msub> <mo>|</mo> <mrow> <mi>x</mi> <mo>=</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> </mrow> </msub> <mo>=</mo> <mi>M</mi> <mrow> <mo>(</mo> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mo>,</mo> <mover> <mi>p</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mover> <mi>p</mi> <mo>^</mo> </mover> <mi>x</mi> </msub> <msub> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mi>z</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mover> <mi>p</mi> <mo>^</mo> </mover> <mi>x</mi> </msub> <msub> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mi>y</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>p</mi> <mo>^</mo> </mover> <mi>y</mi> </msub> <msub> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mi>z</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mover> <mi>p</mi> <mo>^</mo> </mover> <mi>y</mi> </msub> <msub> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mi>x</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mover> <mi>p</mi> <mo>^</mo> </mover> <mi>z</mi> </msub> <msub> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mi>y</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msub> <mover> <mi>p</mi> <mo>^</mo> </mover> <mi>z</mi> </msub> <msub> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mi>x</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>

<mrow> <mfrac> <mrow> <mo>&part;</mo> <mi>d</mi> </mrow> <mrow> <mo>&part;</mo> <mi>p</mi> </mrow> </mfrac> <msub> <mo>|</mo> <mrow> <mi>x</mi> <mo>=</mo> <mover> <mi>x</mi> <mo>^</mo> </mover> </mrow> </msub> <mo>=</mo> <mi>N</mi> <mrow> <mo>(</mo> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mi>y</mi> </msub> <msub> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mi>z</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mi>x</mi> </msub> <msub> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mi>z</mi> </msub> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mi>x</mi> </msub> <msub> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mi>y</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>

根据刚体转动的性质，设状态参数中微分不变量为θ，则θ＝[p^T,ρ^T,μ_v ^T]^T，并且由此可得出连续时间状态转移矩阵：

<mfenced open = "" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>F</mi> <mo>=</mo> <mfrac> <mrow> <mo>&part;</mo> <msub> <mi>f</mi> <mi>s</mi> </msub> <mrow> <mo>(</mo> <mi>x</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>&part;</mo> <mi>x</mi> </mrow> </mfrac> <msub> <mo>|</mo> <mrow> <mi>x</mi> <mo>=</mo> <mover> <mi>x</mi> <mo>&CenterDot;</mo> </mover> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>-</mo> <mo>&lsqb;</mo> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mo>&times;</mo> <mo>&rsqb;</mo> </mrow> </mtd> <mtd> <mrow> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>E</mi> <mn>3</mn> </msub> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mn>3</mn> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>6</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <mrow> <mi>M</mi> <mrow> <mo>(</mo> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mo>,</mo> <mover> <mi>p</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mi>N</mi> <mrow> <mo>(</mo> <mover> <mi>&omega;</mi> <mo>^</mo> </mover> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>6</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mn>9</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>9</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>9</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>9</mn> <mo>&times;</mo> <mn>6</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> </mfenced>

将方程写成离散形式，有

X(n+1)＝Φ(n)X(n)+V(n)

其中，V(n)表示离散的过程噪声，Φ(n)表示离散状态转移矩阵；对于时常系统有