CN110672078B - 一种基于地磁信息的高旋弹丸姿态估计方法 - Google Patents

Abstract

本发明提出了一种基于地磁信息的高旋弹丸姿态估计方法。通过一个三轴地磁传感器收集数据，然后将得到的模拟信号通过A/D转换芯片转为数字信号，数字信号进入FPGA芯片，通过其中搭载的无损卡尔曼滤波器消去干扰，将分析得到的数据存储在黑匣子中，最后以消除干扰的信号为基础通过三正交比法估计弹丸的各项姿态角。实际实验的结果表明，本发明提出的方法可以高精度地估算出弹丸的各项姿态角，对实验所处环境的抗干扰性更强，可以应用于广泛的工作条件。

Description

一种基于地磁信息的高旋弹丸姿态估计方法

技术领域

本发明属于旋转物体的姿态测定技术领域，具体涉及一种基于地磁信息的高旋弹丸姿态估计方法。

背景技术

准确迅速地测量校准旋转物体的姿态，对提高弹丸的速度和精度有着非常重要的意义。在实际实验中，待测量物体通常体型较小，以高速飞行于高过载的极端环境中，所以在其他实验中常用的陀螺仪等高精度传感器并不适用，无法精确测得物体的姿态。此时，成本较低、占用空间较小、抗过载能力强的MEMS传感器可以用来完成测量任务。

目前在此基础上，有很多方法可以应用于旋转物体的姿态测量，例如磁力仪，MEMS陀螺仪，全球导航卫星系统定位，高速摄像机，太阳传感器，红外传感器等。磁力仪通过测量磁场强度和方向来测定物体的姿态，可以在特殊方法中作为特定部分使用，但是不能广泛适用于大多数环境。MEMS陀螺仪在通常情况下可以使用，但是当旋转物体转速较快时，无法准确测出物体的姿态。同样，MEMS加速度计通过对重力矢量的感受来测量物理姿态，这在通常环境下很适用，但是当待测物体高速旋转时重力矢量并不能被有效感受，所以也不能单独用于高速旋转物体的姿态测量。全球导航卫星系统能在地球表面或近地空间的任何地点为用户提供全天候的3维坐标和速度以及时间信息，但是需要时间来接收卫星信息很难做到实时测量，且信息传递不够稳定可能会部分丢失。高速摄像机，太阳传感器，红外传感器在合适的环境中会是很好的选择，但是在实际实验中他们很容易受到包括天气、周围环境等在内的各种干扰，测量的准确性很低。

发明内容

本发明的目的在于提供一种基于地磁信息的高旋弹丸姿态估计方法，将磁力传感器和无损卡尔曼滤波器组合在一起，提高了对弹丸姿态估计的精确度和抗干扰性。

本发明所采用的技术方案是：

一种基于地磁信息的高旋弹丸姿态估计方法了，包括以下步骤：

步骤1：计算弹丸理论姿态角；

通过将磁场投影到旋转弹丸动力学模型所在的测量坐标系来获得数据，所述动力学模型如下式：

I为弹丸惯性张量，在弹丸测量坐标系的x、y、z三轴上分量分别为0.16、1.8、1.8kgm²，M为弹丸力矩，( )_b表示在弹体坐标系的投影，

为一个3×3的反对称矩阵；

力矩方程表示为：

a＝10^-4Ns²/m²为转矩系数，b＝10^-4Ns²/m为横向阻尼系数，c＝5×10^-6Ns²/m为滚动阻尼系数，d＝10^-6Ns²/m为玛格努斯力矩系数，Δ为总攻角，V为速度向量；

运动方程表示为：

Q为旋转四元数；

根据公式(1)-(3)计算弹丸理论姿态角偏航角ψ、俯仰角θ以及滚转角φ，分别表示为：

弹体的初始角速度在测量坐标系x、y、z三轴上分量分别为350πRad/s、0Rad/s、0Rad/s，弹体的初始偏航角和俯仰角均为0Rad，弹体在惯性坐标系中的初始角速度在x、y、z三轴上分量分别为1000m/s、0m/s、0m/s；

步骤2：将所述三轴地磁传感器、A/D转换芯片、FPGA芯片和黑匣子安装在待测弹丸上；

步骤3：对待使用的所述三轴地磁传感器进行标定；

步骤4：发射待测弹丸，三轴地磁传感器收集数据；

步骤5：三轴地磁传感器收集的数据通过A/D转换芯片，进入FPGA芯片中；

步骤6：对三轴地磁传感器进行在线校正，通过卡尔曼滤波器消除干扰磁场，将分析结果存入黑匣子中，具体包括：

步骤6.1：确定卡尔曼滤波器的状态方程和测量方程：

三轴地磁传感器的实际测量值由下式表示；

(H_m)_m＝(H_t)_m+(H_r)_m； (5)

其中H_m为三轴地磁传感器的实际测量值，H_t为地磁场的真实值，H_r为干扰磁场，符号( )_m表示在测量坐标系的投影，在测量坐标系和弹体坐标系重合的情况下，上式(5)转化为：

(H_m)_m＝T_bi(H_t)_i+(H_r)_b； (6)

符号( )_b表示在弹体坐标系的投影，符号( )_i表示在惯性坐标系的投影，T_bi为从惯性坐标系到弹体坐标系的传递矩阵；

方程(6)两边对时间微分，在干扰磁场短时间内不发生巨大变化的情况下，其时间微分为0，得到以下方程：

为一个3×3的反对称矩阵，有三个独立的非零分量，对传递矩阵T_bi分解重组得：

T_βT_α(H_t)_i即为地磁场在弹体坐标系中的投影，弹体主轴固定的情况下，弹体在径向上的角速度为零，所以矩阵

第一行与第一列均为0，则矩阵第一个分量为0，第二和第三个分量为调和函数，因此(H_m)_m在横向上的真实值表示为两个互补的正弦信号，横向上的真实值的离散状态方程如下：

x₁和x₂是测量坐标系中径向上的三轴地磁传感器的两个输出，x₃为滚转角速度，j和ω₀分别为弹体运动的振幅和相位，得到下面两式：

式(10)和(11)分别为推导到第k步及第k+1步得到的方程，t_k+1＝t_k+Δt，Δt为采样间隔，将式(11)右边的三角函数展开，得到；

用常数和比例的乘积代表x₃，得到；

常数ω＝350π，

是ω的比例；

短时间内弹体的滚转角速度和干扰磁场的强度保持不变，因此第k步和第k+1步中x₃、x₄和x₅的值分别相等，得到：

x₄和x₅是干扰磁场在径向上的两个分量；

式(13)、(14)即为卡尔曼滤波器的状态方程F(x)；

y₁和y₂为接收到的测量坐标系中径向上的两个输出，v₁和v₂为测量坐标系中径向上的两个测量噪声，式(15)即为卡尔曼滤波器的测量方程H(x)；

步骤6.2：对三轴地磁传感器进行在线校正，通过卡尔曼滤波器消除干扰磁场，包括以下步骤：

步骤6.2.1：确定性采样点更新，确定性采样点更新中无损变换的参数分别为：α＝10^-3，μ＝9×10^-3，β＝2，κ＝-2；

步骤6.2.2：时间更新，设定状态变量矩阵x₀＝[x₁,x₂,x₃,x₄,x₅]^T＝[1,0,0,0,0]^T；状态变量的初始协方差矩阵P₀＝diag[1,1,1,1,1]；过程噪声初始协方差矩阵P_v为diag[5×10^-4,5×10^-4,2×10^-3,5×10^-3,5×10^-3]；测量噪声初始协方差矩阵P_w为diag[2.5×10^-4,2.5×10^-4]；

i为当前时刻，从0逐渐更新至2n，n为状态变量数量，取值为5，χ_i为i时刻的确定性采样点，f()为将上一时刻i-1的状态映射到当前i时刻状态的非线性函数，

为过程相对于前一时刻i的状态变量的先验估计；

为状态变量的均值，w_m,i为平均加权系数；

步骤6.2.3：测量更新；

为过程相对于前一时刻i的观测变量的先验估计，h()为反映了观测变量和状态变量间关系的非线性函数；

为观测变量的均值；

P_yy为观测变量的协方差矩阵的先验估计，P_w为测量噪声协方差矩阵；

步骤6.2.4：衰落因子计算；

y_k为k时刻的观测变量值，v为k时刻的误差因子；

C_k为k时刻的新息矩阵，λ为控制新息矩阵更新权重的权重系数；

δ为k时刻的衰落因子，δ₀为δ的初始值；

步骤6.2.5：卡尔曼滤波器数据更新；

P_v为过程噪声初始协方差矩阵，P_xx为状态变量的协方差矩阵的先验估计，w_c,i为方差加权系数；

P_xy为状态变量与观测变量的联合协方差矩阵的先验估计；

P_yy为观测变量的协方差矩阵的先验估计；

B为卡尔曼增益；

x_k为k时刻的状态变量；

P_k＝P_xx-BP_yyB^T； (30)

P_k为k时刻的状态变量的协方差矩阵；

此时即可求出k时刻的各状态变量x₁至x₅，从而能够得到径向上消除干扰后的两个信号分量(x₁-x₄)和(x₂-x₅)及此时的滚转角速度x₃；

步骤7：以步骤6最后求出的消除干扰后的信号分量为基础，通过三正交比法确定弹丸姿态角。

相比于现有技术，本发明的有益效果是：本发明具有旋转弹丸姿态估计功能，可以同时满足精确性和稳定性的要求；结构简单易于实现，只需一个三轴地磁传感器、A/D转换芯片及一个用于构建的卡尔曼滤波器的FPGA芯片；精确度高，实际实验得到的估计姿态与实际数据差距较小；稳定性好，对弹丸进行了在线校准，可以较好地消除干扰磁场的影响。

附图说明

图1是实现本发明基于地磁信息的高旋弹丸姿态估计方法的构成示意图。

图2是本发明基于地磁信息的高旋弹丸姿态估计方法的流程示意图。

图3是本发明计算得出的弹丸理论运动偏航角变化曲线图。

图4是本发明计算得出的弹丸理论运动俯仰角变化曲线图。

图5是本发明计算得出的弹丸理论运动姿态图。

图6是本发明弹丸未受干扰前地磁传感器x轴输出曲线图。

图7是本发明弹丸未受干扰前实际运动姿态图。

图8是本发明弹丸受干扰后地磁传感器x轴输出曲线图。

图9是本发明弹丸受干扰后实际运动姿态图。

图10是本发明得到的滚转角速度与理论值对比图。

图11是本发明得到的地磁场强度在弹体坐标系中的投影长度与理论值对比图。

具体实施方式

下面结合附图和具体实施方式对本发明进行详细说明。

本发明基于地磁信息的高旋弹丸姿态估计方法的实现结构组成示意图如图1，图2给出了本发明基于地磁信息的高旋弹丸姿态估计方法的流程示意图，通过三轴地磁传感器1收集模拟信号数据，然后将得到的模拟信号通过A/D转换芯片2转为数字信号，再将数字信号通过FPGA芯片3，通过其中设定的无损卡尔曼滤波器消去干扰，将消去干扰得到的数据存储在黑匣子4中，最后以消除干扰的数据为基础通过三正交比法等方法估计弹丸的各项姿态角。下面给出具体实现步骤。

步骤1：计算弹丸理论姿态角，作为实验参照的理论数据。

通过将磁场投影到旋转弹丸动力学模型所在的测量坐标系来获得数据。动力学模型如下式：

I为弹丸惯性张量，在测量坐标系的x、y、z三轴上分量分别为0.16、1.8、1.8kgm²。M为弹丸力矩，( )_b表示在弹体坐标系的投影，

为一个3×3的反对称矩阵。

力矩方程可表示为：

a＝10^-4Ns²/m²为转矩系数，b＝10^-4Ns²/m为横向阻尼系数，c＝5×10^-6Ns²/m为滚动阻尼系数，d＝10^-6Ns²/m为玛格努斯力矩系数，Δ为总攻角，V为速度向量。

运动方程可表示为：

Q为旋转四元数；

根据公式(1)-(3)计算弹丸理论姿态角偏航角ψ、俯仰角θ以及滚转角φ，分别表示为：

弹体的初始角速度在测量坐标系x、y、z三轴上分量分别为350πRad/s、0Rad/s、0Rad/s。弹体的初始偏航角和俯仰角均为0Rad。弹体在惯性坐标系中的初始角速度在x、y、z三轴上分量分别为1000m/s、0m/s、0m/s。

经计算得到的弹丸理论运动姿态如图3-5所示。图3-5分别为弹丸的偏航角、俯仰角及实际运动状态示意图。图3的y轴为偏航角角度，图4的y轴为俯仰角角度，图5的x轴为偏航角角度、y轴为俯仰角角度，图5整体可看做是弹体实际运动的轨迹示意图。

采用零均值均匀分布的方法，对坐标系中的干扰磁场进行随机补强，互补值每两秒随机变化一次，还考虑了具有零均值和0.05方差的高斯分布的热噪声的影响，干扰磁场强度与地磁场比值为1。

未受干扰磁场影响的实际实验数据如图6-7,受干扰磁场影响的数据如图8-9。图6与图8中y轴表示安装在磁力传感器x轴上的输出在弹体坐标系中的数据，图7与图9为弹体的实际运动轨迹图，图7和图9的x、y轴分别代表弹体的偏航角和俯仰角，整幅图可以看做是弹体实际运动的轨迹示意图，可以看出受到干扰的数据明显无法用于后续的姿态估算实验。

步骤2：将三轴地磁传感器1、A/D转换芯片2、FPGA芯片3和黑匣子4安装在待测弹丸上。

步骤3：对待使用的三轴地磁传感器1进行标定。

步骤4：发射待测弹丸，三轴地磁传感器1收集数据。

步骤5：三轴地磁传感器(1)收集的数据通过A/D转换芯片2，进入FPGA芯片3中。

步骤6：对三轴地磁传感器1进行在线校正，通过卡尔曼滤波器消除干扰磁场，将分析结果存入黑匣子4中，具体包括：

步骤6.1：确定卡尔曼滤波器的状态方程和测量方程：

三轴地磁传感器1的实际测量值可由下式表示；

(H_m)_m＝(H_t)_m+(H_r)_m； (5)

其中H_m为三轴地磁传感器1的实际测量值，H_t为地磁场的真实值，H_r为干扰磁场，符号( )_m表示在测量坐标系的投影。在测量坐标系和弹体坐标系重合的情况下，上式可转化为：

(H_m)_m＝T_bi(H_t)_i+(H_r+)_b； (6)

符号( )_b表示在弹体坐标系的投影，符号( )_i表示在惯性坐标系的投影，T_bi为从惯性坐标系到弹体坐标系的传递矩阵。

方程(6)两边对时间微分，在干扰磁场在短时间内不发生巨大变化的情况下，其时间微分为0，可得到下述方程：

为一个3×3的反对称矩阵，有三个独立的非零分量，对传递矩阵T_bi分解重组可得：

T_βT_α(H_t)_i即为地磁场在弹体坐标系中的投影。弹体主轴固定情况下，弹体在径向上的角速度为零，所以矩阵

第一行与第一列均为0，则矩阵第一个分量为0，第二和第三个分量为调和函数。这表明横向的真实数据是谐波，轴向的真实数据是恒定的。所以(H_m)_m在横向上的真实值可以表示为两个互补的正弦信号。横向上的真实值的离散状态方程如下：

x₁和x₂是测量坐标系中径向上的三轴地磁传感器1的两个输出。x₃为滚转角速度。j和ω₀分别为弹体运动的振幅和相位。可以得到下面两式；

式(10)和(11)分别为推导到第k步及第k+1步得到的方程，t_k+1＝t_k+Δt，Δt为采样间隔。将式(11)右边的三角函数展开，可得到；

用常数和比例的乘积代表x₃，可得到；

常数ω＝350π，

是ω的比例，将比例选择为指数函数可使它的值始终大于零。

短时间内，弹体的滚转角速度和干扰磁场的强度可看做是不变的，所以第k步和第k+1步中x₃、x₄和x₅的值分别相等。

x₄和x₅是干扰磁场在径向上的两个分量。

式(13)、(14)即为卡尔曼滤波器的状态方程F(x)。

y₁和y₂为接收到的测量坐标系中径向上的两个输出。v₁和v₂为测量坐标系中径向上的两个测量噪声。式(15)即为卡尔曼滤波器的测量方程H(x)。

步骤6.2：对三轴地磁传感器1进行在线校正，通过卡尔曼滤波器消除干扰磁场，包括以下步骤：

步骤6.2.1：确定性采样点更新，确定性采样点更新中无损变换的参数分别为：α＝10^-3，μ＝9×10^-3，β＝2，κ＝-2。

步骤6.2.2：时间更新，设定状态变量矩阵x₀＝[x₁,x₂,x₃,x₄,x₅]^T＝[1,0,0,0,0]^T；状态变量的初始协方差矩阵P₀＝diag[1,1,1,1,1]；过程噪声初始协方差矩阵P_v为diag[5×10^-4,5×10^-4,2×10^-3,5×10^-3,5×10^-3]；测量噪声初始协方差矩阵P_w为diag[2.5×10^-4,2.5×10^-4]；

i为当前时刻，从0逐渐更新至2n，n为状态变量数量，取值为5，χ_i为i时刻的确定性采样点，f()为将上一时刻i-1的状态映射到当前i时刻状态的非线性函数，

为过程相对于前一时刻i的状态变量的先验估计。

为状态变量的均值，w_m,i为平均加权系数。

步骤6.2.3：测量更新；

为过程相对于前一时刻i的观测变量的先验估计，h()为反映了观测变量和状态变量间关系的非线性函数。

为观测变量的均值。

P_yy为观测变量的协方差矩阵的先验估计，P_w为测量噪声协方差矩阵。

步骤6.2.4：衰落因子计算；

y_k为k时刻的观测变量值，v为k时刻的误差因子。

C_k为k时刻的新息矩阵，λ为控制新息矩阵更新权重的权重系数。

δ为k时刻的衰落因子，δ₀为δ的初始值。

步骤6.2.5：卡尔曼滤波器数据更新；

P_v为过程噪声初始协方差矩阵，P_xx为状态变量的协方差矩阵的先验估计，w_c,i为方差加权系数。

P_xy为状态变量与观测变量的联合协方差矩阵的先验估计。

P_yy为观测变量的协方差矩阵的先验估计。

B为卡尔曼增益。

x_k为k时刻的状态变量。

P_k＝P_xx-BP_yyB^T； (30)

P_k为k时刻的状态变量的协方差矩阵；

此时即可求出k时刻的各状态变量x₁至x₅，故可以得到径向上消除干扰后的两个信号分量(x₁-x₄)和(x₂-x₅)及此时的滚转角速度x₃。

图10为得到的滚转角速度与理论值对比图，其中y轴为滚转角速度，虚线为实验值，实线为理论值。图11为得到的地磁场强度在弹体坐标系中的投影(即消除干扰后的两个信号分量的和向量)长度与理论值对比图，其中y轴为投影长度，虚线为实验值，实线为理论值。可以看到两张图中约2秒后理论值与实验值均能很好地收敛，可以正确估计滚转角速度和地磁场强度在弹体坐标系中的投影长度，收敛后估计值与理论值基本一致，可以很好地消除干扰磁场的影响。

步骤7：以消除了干扰的信号分量为基础，通过三正交比法确定弹丸姿态角。以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种基于地磁信息的高旋弹丸姿态估计方法，其特征在于包括以下步骤：

步骤1：计算弹丸理论姿态角；

通过将磁场投影到旋转弹丸动力学模型所在的测量坐标系来获得数据，所述动力学模型如下式：

I为弹丸惯性张量，在弹丸测量坐标系的x、y、z三轴上分量分别为0.16、1.8、1.8kgm²，M为弹丸力矩，()_b表示在弹体坐标系的投影，

为一个3×3的反对称矩阵；

力矩方程表示为：

a＝10^-4Ns²/m²为转矩系数，b＝10^-4Ns²/m为横向阻尼系数，c＝5×10^-6Ns²/m为滚动阻尼系数，d＝10^-6Ns²/m为玛格努斯力矩系数，Δ为总攻角，V为速度向量；

运动方程表示为：

Q为旋转四元数；

根据公式(1)-(3)计算弹丸理论姿态角偏航角ψ、俯仰角θ以及滚转角φ，分别表示为：

弹体的初始角速度在测量坐标系x、y、z三轴上分量分别为350πRad/s、0Rad/s、0Rad/s，弹体的初始偏航角和俯仰角均为0Rad，弹体在惯性坐标系中的初始角速度在x、y、z三轴上分量分别为1000m/s、0m/s、0m/s；

步骤2：将三轴地磁传感器(1)、A/D转换芯片(2)、FPGA芯片(3)和黑匣子(4)安装在待测弹丸上；

步骤3：对待使用的所述三轴地磁传感器(1)进行标定；

步骤4：发射待测弹丸，所述三轴地磁传感器(1)收集数据；

步骤5：所述三轴地磁传感器(1)收集的数据通过A/D转换芯片(2)，进入FPGA芯片(3)中；

步骤6：对所述三轴地磁传感器(1)进行在线校正，通过卡尔曼滤波器消除干扰磁场，将分析结果存入黑匣子(4)中，具体包括：

步骤6.1：确定卡尔曼滤波器的状态方程和测量方程：

所述三轴地磁传感器(1)的实际测量值由下式表示；

(H_m)_m＝(H_t)_m+(H_r)_m； (5)

其中H_m为所述三轴地磁传感器(1)的实际测量值，H_t为地磁场的真实值，H_r为干扰磁场，符号()_m表示在测量坐标系的投影，在测量坐标系和弹体坐标系重合的情况下，上式(5)转化为：

(H_m)_m＝T_bi(H_t)_i+(H_r)_b； (6)

符号()_b表示在弹体坐标系的投影，符号()_i表示在惯性坐标系的投影，T_bi为从惯性坐标系到弹体坐标系的传递矩阵；

方程(6)两边对时间微分，在干扰磁场短时间内不发生巨大变化的情况下，其时间微分为0，得到以下方程：

为一个3×3的反对称矩阵，有三个独立的非零分量，对传递矩阵T_bi分解重组得：

T₂T₁(H_t)_i即为地磁场在弹体坐标系中的投影，弹体主轴固定的情况下，弹体在径向上的角速度为零，所以矩阵

第一行与第一列均为0，则矩阵第一个分量为0，第二和第三个分量为调和函数，因此(H_m)_m在横向上的真实值表示为两个互补的正弦信号，横向上的真实值的离散状态方程如下：

x₁和x₂是测量坐标系中径向上的所述三轴地磁传感器(1)的两个输出，x₃为滚转角速度，j和ω₀分别为弹体运动的振幅和相位，得到下面两式：

式(10)和(11)分别为推导到第k步及第k+1步得到的方程，t_k+1＝t_k+Δt，Δt为采样间隔，将式(11)右边的三角函数展开，得到；

用常数和比例的乘积代表x₃，得到；

常数ω＝350π，

是ω的比例；

短时间内弹体的滚转角速度和干扰磁场的强度保持不变，因此第k步和第k+1步中x₃、x₄和x₅的值分别相等，得到：

x₄和x₅是干扰磁场在径向上的两个分量；

式(13)、(14)即为卡尔曼滤波器的状态方程F(x)；

y₁和y₂为接收到的测量坐标系中径向上的两个输出，v₁和v₂为测量坐标系中径向上的两个测量噪声，式(15)即为卡尔曼滤波器的测量方程H(x)；

步骤6.2：对所述三轴地磁传感器(1)进行在线校正，通过卡尔曼滤波器消除干扰磁场，包括以下步骤：

步骤6.2.1：确定性采样点更新，确定性采样点更新中无损变换的参数分别为：α＝10^-3，μ＝9×10^-3，β＝2，κ＝-2；

步骤6.2.2：时间更新，设定状态变量矩阵x₀＝[x₁，x₂，x₃，x₄，x₅]^T＝[1，0，0，0，0]^T；状态变量的初始协方差矩阵P₀＝diag[1，1，1，1，1]；过程噪声初始协方差矩阵P_v为diag[5×10^-4，5×10^-4，2×10^-3，5×10^-3，5×10^-3]；测量噪声初始协方差矩阵P_w为diag[2.5×10^-4，2.5×10^-4]；

i为当前时刻，从0逐渐更新至2n，n为状态变量数量，取值为5，χ_i为i时刻的确定性采样点，f()为将上一时刻i-1的状态映射到当前i时刻状态的非线性函数，

为过程相对于前一时刻i的状态变量的先验估计；

为状态变量的均值，w_m,i为平均加权系数；

步骤6.2.3：测量更新；

为过程相对于前一时刻i的观测变量的先验估计，h()为反映了观测变量和状态变量间关系的非线性函数；

为观测变量的均值；

P_yy为观测变量的协方差矩阵的先验估计，P_w为测量噪声协方差矩阵；

步骤6.2.4：衰落因子计算；

y_k为k时刻的观测变量值，v为k时刻的误差因子；

C_k为k时刻的新息矩阵，λ为控制新息矩阵更新权重的权重系数；

δ为k时刻的衰落因子，δ₀为δ的初始值；

步骤6.2.5：卡尔曼滤波器数据更新；

P_v为过程噪声初始协方差矩阵，P_xx为状态变量的协方差矩阵的先验估计，w_c,i为方差加权系数；

P_xy为状态变量与观测变量的联合协方差矩阵的先验估计；

P_yy为观测变量的协方差矩阵的先验估计；

B为卡尔曼增益；

x_k为k时刻的状态变量；

P_k＝P_xx-BP_yyB^T； (30)

P_k为k时刻的状态变量的协方差矩阵；

此时即可求出k时刻的各状态变量x₁至x₅，从而能够得到径向上消除干扰后的两个信号分量(x₁-x₄)和(x₂-x₅)及此时的滚转角速度x₃；

步骤7：以步骤6最后求出的消除干扰后的信号分量为基础，通过三正交比法确定弹丸姿态角。

Images