CN103727940B - 基于重力加速度矢量匹配的非线性初始对准方法 - Google Patents

Abstract

本发明提供了一种基于重力加速度矢量匹配的非线性初始对准方法。主要步骤包括：捷联惯性导航系统粗对准；建立基于导航坐标系重力加速度矢量匹配的非线性滤波连续系统模型及其离散化；采用容积卡尔曼非线性滤波器进行滤波，完成初始对准。优点在于本发明中状态量采用欧拉角和惯性器件常值误差，以欧拉角微分方程和惯性器件误差模型为状态方程，量测量采用导航坐标系中的重力加速度矢量，以速度微分方程为量测方程，并采用容积卡尔曼非线性滤波算法进行滤波估计，滤波输出直接为导航参数，在滤波时间更新过程中同步实现捷联惯导系统的导航参数更新，不需要单独的导航解算过程，算法非常简单，为捷联惯性导航系统初始对准提供了一种新方案。

Description

基于重力加速度矢量匹配的非线性初始对准方法

技术领域

本发明涉及一种基于重力加速度矢量匹配的非线性初始对准方法，用于确定捷联惯性导航系统的初始姿态，属于导航、制导与控制技术领域。

背景技术

捷联惯性导航系统中惯性测量单元主要包含三只陀螺仪和三只加速度计，通过对陀螺仪测得的角速度信息和加速度计测得的线加速度信息进行积分来获得载体位置、速度和姿态等导航信息。根据这个原理，捷联惯性导航系统在导航之前必须获得初始导航信息，这个过程即为初始对准，其主要任务是初始姿态的确定，找到数学平台。

初始对准分为粗对准和精对准两个阶段。粗对准阶段，根据惯性器件测得的角速度信息和线加速度信息，采用解析法或者凝固等粗对准算法粗略的获得初始姿态角；精对准阶段，常采用系统误差模型和相应的滤波算法，以速度误差、位置误差以及它们的组合等为量测量，通过各种匹配方式来精确估计出误差量，并用这些误差量修正导航参数，提高对准精度。

本发明中利用初始对准时载体线速度一般为零的约束条件，量测量采用导航坐标系中的重力加速度矢量，状态量采用欧拉角和惯性器件常值误差，以欧拉角微分方程和惯性器件误差模型为状态方程，以速度微分方程为量测方程，建立基于重力加速度矢量匹配的非线性直接法初始对准模型，其状态方程和量测方程均为非线性；采用容积卡尔曼非线性滤波算法进行滤波估计，滤波时间更新过程中同步实现捷联惯导系统的导航参数更新，不需要单独的导航解算过程，算法非常简单。

发明内容

本发明的目的在于不同于传统的捷联惯性导航系统误差模型，直接以其欧拉角微分方程和惯性器件误差模型为状态方程，以其速度微分方程为量测方程，状态量采用欧拉角和惯性器件常值误差，量测量采用导航坐标系中的重力加速度矢量，利用容积卡尔曼非线性滤波方法来实现初始对准，精确确定初始的姿态信息，为捷联惯性导航系统的初始对准提供一种新方法。

本发明主要包含以下步骤：

步骤1、捷联惯性导航系统粗对准：根据惯性器件陀螺仪测得的角速度信息和加速度计测得的线加速度信息，忽略惯性器件的测量误差，使用基于惯性系重力加速度的凝固解析粗对准算法求得粗略的初始纵摇角θ、横摇角γ和航向角ψ；

步骤2、在载体线速度为零的前提下，根据捷联惯性导航系统的欧拉角微分方程、惯性器件误差模型以及以导航坐标系重力加速度矢量为量测量的量测方程，选定9维系统状态量和3维量测量，建立基于重力加速度矢量匹配的非线性连续系统模型，其状态方程和量测方程均为非线性；

步骤3、将步骤2中建立的非线性连续系统模型离散化，形成非线性离散系统模型，非线性离散系统模型的状态方程与量测方程均为非线性；

步骤4、针对步骤3中建立的非线性离散系统模型，采用容积卡尔曼非线性滤波器进行滤波，滤波周期与惯性器件的数据更新周期一致，周期为T_f。

本发明还可以包括这样一些特征：

1.所述的步骤2在载体线速度为零的前提下，根据捷联惯性导航系统的欧拉角微分方程、惯性器件误差模型以及以导航坐标系重力加速度矢量为量测量的量测方程，选定9维系统状态量和3维量测量，建立基于重力加速度矢量匹配的非线性连续系统模型，其状态方程和量测方程均为非线性，具体为：

以东北天地理坐标系作为导航坐标系-n系，以载体右前上方向矢量右手定则构成的坐标系作为载体坐标系-b系，假设载体线速度为零。

捷联惯性导航系统的欧拉角微分方程为：

$\left[\begin{array}{c}\stackrel{.}{\mathrm{\θ}}\\ \stackrel{.}{\mathrm{\γ}}\\ \stackrel{.}{\mathrm{\ψ}}\end{array}\right]=\frac{1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}& -\cos \mathrm{\γ}\sin \mathrm{\θ}\\ -\sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]{\mathrm{\ω}}_{\mathrm{nb}}^b$

式中，θ为纵摇角；γ为横摇角；ψ为航向角；为载体坐标系相对导航坐标系的角速度矢量在载体坐标系轴向的分量构成的列矩阵，在载体线速度为零的情况下，为载体坐标系轴向三只陀螺仪无误差的测量值， ${\mathrm{\ω}}_{\mathrm{ib}}^b={{[\widetilde{\mathrm{\ω}}}_{\mathrm{ibx}}^b-\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ibx}}^b,{\widetilde{\mathrm{\ω}}}_{\mathrm{iby}}^b,{\widetilde{\mathrm{\ω}}}_{\mathrm{ibz}}^b-\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ibz}}^b]}^T,$ 和为载体坐标系轴向三只陀螺仪测量值，和为三只陀螺仪相应的测量误差，

ε_gx、ε_gy和ε_gz为三只陀螺仪测量误差的随机常值部分，w_gx、w_gy和w_gz为三只陀螺仪测量误差的随机噪声部分，随机噪声均假设为零均值高斯白噪声；为姿态矩阵，

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}& \mathrm{ocs\γ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\θ}\cos \mathrm{\γ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\θ}\cos \mathrm{\γ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right];$

为地球自转角速度在导航坐标系中的投影，ω_ie为地球自转角速度，L为当地地理纬度；

捷联惯性导航系统的惯性器件测量误差的随机常值部分模型为：

${\stackrel{.}{\▿}}_{\mathrm{ax}}=0,{\stackrel{.}{\▿}}_{\mathrm{ay}}=0,{\stackrel{.}{\▿}}_{\mathrm{az}}=0,{\stackrel{.}{\mathrm{\ϵ}}}_{\mathrm{gx}}=0,{\stackrel{.}{\mathrm{\ϵ}}}_{\mathrm{gy}}=0{\stackrel{.}{,\mathrm{\ϵ}}}_{\mathrm{gz}}=0;$

选定的系统状态向量为：

$\mathrm{\θ},\mathrm{\γ},\mathrm{\ψ},{\▿}_{\mathrm{ax}},{\▿}_{\mathrm{zy}},{\▿}_{\mathrm{az}},{\mathrm{\ϵ}}_{\mathrm{gx}},{\mathrm{\ϵ}}_{\mathrm{gy}},{\mathrm{\ϵ}}_{\mathrm{gz}}{]}^T$

式中，x为系统状态向量，9维；

系统噪声向量为：

w＝[w_gx,w_gy,w_gz,0,0,0,0,0,0]^T

式中，w为系统噪声向量；

在载体线速度为零的情况下，以导航系重力加速度矢量为量测量的量测方程为：

gⁿ＝-fⁿ

式中，gⁿ为重力加速度矢量在n系中投影，其中gⁿ=[0,0,-g]^T，g为重力加速度；fⁿ为加速度计无误差的测量值在n系中投影，其中 为载体系坐标轴方向三只加速度计无误差的测量值，其中 $f_{\mathrm{ib}}^b=[{\tilde{f}}_{\mathrm{ibx}}^b-{\mathrm{\δf}}_{\mathrm{ibx}}^b,{\tilde{f}}_{\mathrm{iby}}^b-{\mathrm{\δf}}_{\mathrm{iby}}^b,{\tilde{f}}_{\mathrm{ibz}}^b-{\mathrm{\δf}}_{\mathrm{ibz}}^b{]}^T,$ 和分别为三只加速度计相应的测量值；和分别为三只加速度计相应的测量误差，其中 和为三只加速度计测量误差的随机常值部分，w_ax、w_ay和w_az为三只加速度计测量误差的随机噪声部分，随机噪声均假设为零均值高斯白噪声；

选择gⁿ为量测量，则3维量测量为：

z＝[0,0,-g]^T

式中，z为量测向量；

量测噪声向量为：

η＝[w_ax,w_ay,w_az]^T

式中，η为量测噪声向量；

根据捷联惯性导航系统的欧拉角微分方程、惯性器件误差模型以及量测方程建立非线性滤波连续系统模型如下：

$\left\{\begin{array}{c}\stackrel{.}{x}=F\left(x\right)+G\left(x\right)w\\ z=H\left(x\right)+U\left(x\right)\mathrm{\η}\end{array}\right.$

式中，F(x)为非线性状态转移函数，根据选定的状态向量参照欧拉角微分方程和惯性器件误差模型写出；H(x)为非线性量测函数，根据选定的状态向量和量测向量参照量测方程写出；G(x)为系统噪声输入函数，根据选定的状态向量和系统噪声向量参照欧拉角微分方程写出；U(x)为量测噪声输入函数，根据选定的量测向量和量测噪声向量参照量测方程写出。

2.所述的步骤3中将步骤2中建立的非线性连续系统模型离散化，得到非线性离散系统模型，非线性离散系统模型为：

$\left\{\begin{array}{c}{x}_k=F\left(x_{k-1}\right)+w_{k-1}\\ z_k=H\left(x_k\right)+{\mathrm{\η}}_k\end{array}\right.$

式中，x_k和x_k-1分别为t_k和t_k-1时刻的系统状态向量；z_k为t_k时刻的量测向量；F(x_k-1)为t_k-1时刻的非线性状态转移函数，离散化时，采用四阶龙格库塔方法；H(x_k)为t_k时刻的非线性量测函数；w_k和w_k-1分别为t_k和t_k-1时刻的系统噪声向量；η_k为t_k时刻的量测噪声向量，且w_k和η_k满足：

$\left\{\begin{array}{c}E\left[w_k\right]=0,E\left[w_k{w}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[{\mathrm{\η}}_k\right]=0,E\left[{\mathrm{\η}}_k{\mathrm{\η}}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[w_k{\mathrm{\η}}_j^T\right]=0\end{array}\right.$

式中，δ_kj是δ函数；

离散系统噪声方差强度阵为：

Q_k＝G(x_k)QG(x_k)^TT_f

式中，Q_k为离散系统噪声w_k的方差强度阵；G(x_k)为t_k时刻的系统噪声输入函数；Q为连续系统噪声w的方差强度阵；T_f为滤波周期；

离散量测噪声方差强度阵为：

R_k＝U(x_k)RU(x_k)^T/T_f

式中，R_k为离散系统噪声η_k的方差强度阵；U(x_k)为t_k时刻的系统量测噪声输入函数；R为连续系统噪声η的方差强度阵。

3.所述的步骤4中容积卡尔曼非线性滤波器的滤波步骤为：

1）初始化系统状态向量及其方差矩阵：

${\hat{x}}_0=E\left(x_0\right),P_0=E\left(({\hat{x}}_0-x_0){({\hat{x}}_0-x_0)}^T\right)$

式中，和P₀分别为t₀时刻的系统状态向量估计值及其方差阵；

2）时间更新：

分解前一步的方差阵：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T$

式中，P_k-1|k-1和S_k-1|k-1分别为t_k-1时刻的系统状态向量方差阵及其平方根阵；

计算状态容积点：

$X_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}$

式中，χ_i,k-1|k-1为t_k-1时刻的系统状态向量的第i个容积点；为t_k-1时刻的系统状态向量估计值；ξ_i为容积卡尔曼第i个容积点；

计算非线性状态函数传播的容积点：

$X_{i,k|k-1}^{*}=F\left(X_{i,k-1|k-1}\right)$

式中，为χ_i,k-1|k-1通过非线性状态函数F(χ_i,k-1|k-1)传播的容积点；

计算状态的一步预测值：

${\hat{x}}_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}{a}_i$

式中，为一步预测的t_k时刻系统状态向量;a_i为容积点ξ_i对应的权重；m为容积点总个数；i为容积点的序号；

计算一步预测方差阵：

$P_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{a}_i{X}_{i,k|k-1}^{*}{X}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}^T+Q_{k-1}$

式中，P_k|k-1为一步预测的tk时刻系统状态向量方差阵;Q_k-1为t_k-1时刻的系统噪声方差强度阵；

3）量测更新：

分解一步预测方差阵：

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T$

式中，S_k|k-1为一步预测的t_k时刻系统状态向量方差阵的平方根阵；

计算一步预测容积点：

$X_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1}$

式中，χ_i,k|k-1为一步预测的tk时刻系统状态向量的第i个容积点；

计算通过非线性量测函数传播的量测容积点：

Z_i,k|k-1＝H(χ_i,k|k-1)

式中，Z_i,k|k-1为χ_i,k|k-1通过非线性量测函数H(χ_i,k|k-1)传播的容积点，即一步预测的t_k时刻系统量测向量的第i个容积点；

计算一步预测量测值：

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{Z}_{i,\mathrm{ki}|k-1}{a}_i$

式中，为一步预测的t_k时刻量测向量；

计算一步预测量测方差阵：

$P_{\mathrm{zz},k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{a}_i{Z}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k$

式中，P_zz,k|k-1为一步预测的t_k时刻量测方差阵；

计算一步预测协方差阵：

$P_{\mathrm{xz},k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{a}_i{X}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T$

式中，P_xz,k|k-1为一步预测的tk时刻协方差阵；

计算卡尔曼滤波增益：

$K_k=P_{\mathrm{xz},k|k-1}{P}_{\mathrm{zz},k|k-1}^{-1}$

式中，K_k为t_k时刻卡尔曼滤波增益阵；

更新状态：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

更新状态方差阵：

$P_{k|k}=P_{k|k-1}-K_k{P}_{\mathrm{zz},k|k-1}{K}_k^T.$

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

本发明的优点在于：

1）本发明中利用初始对准时载体线速度一般为零的约束条件，建立基于导航坐标系重力加速度矢量匹配的直接法初始对准模型，较传统的误差模型，模型简单直接，无任何近似，更准确；

2）本发明中初始对准模型的状态方程和量程方程均为非线性，采用了容积卡尔曼非线性滤波算法，较传统的线性卡尔曼滤波，适用范围更广；

3）本发明中以欧拉角微分方程和惯性器件误差模型为状态方程，以速度微分方程为量测方程，滤波时间更新过程中同步实现导航参数更新，较传统的初始对准方法，不需要单独的导航解算过程；

因此本发明为捷联惯性导航系统初始对准提供了一种新的非线性解决方案，可以精确估计出初始的姿态欧拉角，为后续的导航提供精确的数学平台。

本发明提出的方案通过如下半物理试验加以验证：

1）本试验数据来源于某型惯性测量单元的转台实测数据，转台为三轴摇摆，晃动频率均为0.2Hz，晃动幅值均为5°，转台最终停在航向为0°的位置，以检验初始对准的精度。

2）传感器数据采样时间为5ms，滤波周期T_f为5ms，仿真时间半小时，前面5分钟用于粗对准；

3）三只加速度计随机常值均设为0.2mg，随机噪声均设0.2mg，三只陀螺仪随机常值0.03°/h，随机噪声0.03°/h；

4）位置：初始纬度为26.5017°，初始经度为106.71667°；

5）滤波器的初始条件设为

x=[θ,γ,ψ,0,0,0,0,0,0]^T，式中θ，γ，ψ为5分钟粗对准结束时的姿态角；

z=[0,0,-g]^T；

P0=diag{(0.1°)²,(0.1°)²,(0.3°)²,(0.2mg)²,(0.2mg)²,(0.2mg)²,

(0.03°/h)²,(0.03°/h)²,(0.03°/h)²}；

Q=diag{(0.03°/h)²,(0.03°/h)²,0.03°/h)²,0,0,0,0,0,0}；

R=diag{(0.2mg)²,(0.2mg)²,(0.2mg)²}。

式中，diag{.}表示对角矩阵。

通过计算机仿真，摇摆台的初始对准姿态角θ，γ，ψ的曲线如附图3-5所示。对准误差分别为-0.52′，0.48′，11.69′，满足系统要求。

附图说明

图1为本发明实现捷联惯性导航系统初始对准的具体实施方式；

图2为本发明的容积卡尔曼非线性滤波的算法框图；

图3为本发明的半物理仿真初始对准的纵摇角θ的曲线图；

图4为本发明的半物理仿真初始对准的横摇角γ的曲线图；

图5为本发明的半物理仿真初始对准的航向角ψ的曲线图。

具体实施方式

下面结合附图1，通过摇摆转台捷联惯性测量单元实测数据进行半物理仿真试验，对本发明做详尽描述：

步骤1、捷联惯性导航系统粗对准：根据惯性器件陀螺仪测得的角速度信息和加速度计测得的线加速度信息，忽略惯性器件的测量误差，使用基于惯性系重力加速度的凝固解析粗对准算法求得粗略的初始纵摇角θ、横摇角γ和航向角ψ；

步骤2、在载体线速度为零的前提下，根据捷联惯性导航系统的欧拉角微分方程、惯性器件误差模型以及以导航坐标系重力加速度矢量为量测量的量测方程，选定9维系统状态量和3维量测量，建立基于重力加速度矢量匹配的非线性连续系统模型，其状态方程和量测方程均为非线性；

以东北天（east-north-up）地理坐标系作为导航坐标系(n系)，以载体右前上（x-y-z）方向矢量右手定则构成的坐标系作为载体坐标系（b系），假设载体线速度为零。

捷联惯性导航系统的欧拉角微分方程为：

$\left[\begin{array}{c}\stackrel{.}{\mathrm{\θ}}\\ \stackrel{.}{\mathrm{\γ}}\\ \stackrel{.}{\mathrm{\ψ}}\end{array}\right]=\frac{1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}& -\cos \mathrm{\γ}\sin \mathrm{\θ}\\ -\sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]{\mathrm{\ω}}_{\mathrm{nb}}^b---\left(1\right)$

式中，θ为纵摇角；γ为横摇角；ψ为航向角；为载体坐标系相对导航坐标系的角速度矢量在载体坐标系轴向的分量构成的列矩阵， 为载体坐标系轴向三只陀螺仪无误差的测量值，

${\mathrm{\ω}}_{\mathrm{ib}}^b=[{\widetilde{\mathrm{\ω}}}_{\mathrm{ibx}}^b-{\mathrm{\δ\ω}}_{\mathrm{ibx}}^b,{\widetilde{\mathrm{\ω}}}_{\mathrm{iby}}^b-{\mathrm{\δ\ω}}_{\mathrm{iby}}^b,{\widetilde{\mathrm{\ω}}}_{\mathrm{ibz}}^b-{\mathrm{\δ\ω}}_{\mathrm{ibz}}^b{]}^T---\left(2\right)$

和为载体坐标系轴向三只陀螺仪测量值，和为三只陀螺仪相应的测量误差，

${\mathrm{\δ\ω}}_{\mathrm{ibx}}^b={\mathrm{\ϵ}}_{\mathrm{gx}}+w_{\mathrm{gx}},{\mathrm{\δ\ω}}_{\mathrm{iby}}^b={\mathrm{\ϵ}}_{\mathrm{gy}}+w_{\mathrm{gy}},{\mathrm{\δ\ω}}_{\mathrm{ibz}}^b={\mathrm{\ϵ}}_{\mathrm{gz}}+w_{\mathrm{gz}}$

ε_gx、ε_gy和ε_gz为三只陀螺仪测量误差的随机常值部分，w_gx、w_gy和w_gz为三只陀螺仪测量误差的随机噪声部分，随机噪声均假设为零均值高斯白噪声；为姿态矩阵，

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}& \mathrm{ocs\γ}\sin \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\θ}\cos \mathrm{\γ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\θ}\cos \mathrm{\γ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(4\right)$

为地球自转角速度在导航坐标系中的投影，

${\mathrm{\ω}}_{\mathrm{ie}}^n=[0,{\mathrm{\ω}}_{\mathrm{ie}}\cos L,{\mathrm{\ω}}_{\mathrm{ie}}\sin L{]}^T---\left(5\right)$

ω_ie为地球自转角速度，L为当地地理纬度；为由于载体的运动而引起的相对地球的旋转角速度在导航坐标系中的投影，

v_east、v_north和v_up分别为导航坐标系的东向、北向和天向速度，r_M为子午圈曲率半径，r_N为卯酉圈曲率半径；

rM=r_q(1-2ρ+3ρsin²L)，rN=r_q(ρsin²L+1)，r_q为参考椭球赤道平面半径，ρ为地球扁率，在载体线速度为零的情况下，v_east、v_north和v_up均为零，因此那么可以简化为

${\mathrm{\ω}}_{\mathrm{nb}}^b={\mathrm{\ω}}_{\mathrm{ib}}^b-C_n^b{\mathrm{\ω}}_{\mathrm{ie}}^n---\left(6\right)$

将式（3）代入式（2）中，然后将式（2）（4）（5）代入式（6）中，最后将式（6）代入（1）中，可以得到完整的非线性欧拉角微分方程。

捷联惯性导航系统的惯性器件（陀螺仪与加速度计）测量误差的随机常值部分模型为：

选定的系统状态向量为：

式中，x为系统状态向量，9维；

系统噪声向量为：

w＝[w_gx,w_gy,w_gz,0,0,0,0,0,0]^T（9）

式中，w为系统噪声向量；

捷联惯性导航系统的速度微分方程为：

${\stackrel{.}{v}}_{\mathrm{en}}^n=f^n-({2\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×v_{\mathrm{en}}^n+g^n---\left(10\right)$

式中，为导航坐标系中的载体相对地球的运动线速度，

在载体线速度为零的情况下，

$v_{\mathrm{en}}^n={\left[\mathrm{0,0,0}\right]}^T---\left(11\right)$

则

${\stackrel{.}{v}}_{\mathrm{en}}^n=0---\left(12\right)$

将式（11）和（12）代入式（10）中，并移项，捷联惯性导航系统的速度微分方程简化为：

gⁿ＝-fⁿ（13）

式中，gⁿ为重力加速度矢量在n系中投影，

gⁿ=[0,0,-g]^T（14）

g为重力加速度；fⁿ为加速度计无误差的测量值在n系中投影，

$f^n=C_b^n{f}_{\mathrm{ib}}^b---\left(15\right)$

$C_b^n={\left(C_n^b\right)}^T---\left(16\right)$

为载体系坐标轴方向三只加速度计无误差的测量值，

$f_{\mathrm{ib}}^b=[{\tilde{f}}_{\mathrm{ibx}}^b-{\mathrm{\δf}}_{\mathrm{ibx}}^b,{\tilde{f}}_{\mathrm{iby}}^b-{\mathrm{\δf}}_{\mathrm{iby}}^b-{\mathrm{\δf}}_{\mathrm{ibz}}^b{]}^T---\left(17\right)$

和分别为三只加速度计相应的测量值，和分别为三只加速度计相应的测量误差，

和为三只加速度计测量误差的随机常值部分，w_ax、w_ay和w_az为三只加速度计测量误差的随机噪声部分，随机噪声均假设为零均值高斯白噪声；

将式（18）代入式（17）中，然后将式（16）和（17）代入式（15）中，最后将式（15）和（14）代入式（13）中可以得到完整的载体速度为零时的非线性速度微分方程。

选择gⁿ为量测量，则式（13）即为以导航系重力加速度矢量为量测量的量测方程。

3维量测量为：

z＝[0,0,-g]^T（19）

式中，z为量测向量；

量测噪声向量为：

η＝[w_ax,w_ay,w_az]^T（20）

式中，η为量测噪声向量；

根据捷联惯性导航系统的欧拉角微分方程、惯性器件误差模型以及量测方程建立非线性滤波连续系统模型如下：

$\left\{\begin{array}{c}\stackrel{.}{x}=F\left(x\right)+G\left(x\right)w\\ z=H\left(x\right)+U\left(x\right)\mathrm{\η}\end{array}\right.$

式中，F(x)为非线性状态转移函数，根据式（8）选定的状态向量，F(x)参照式（1）和式（7）写出；H(x)为非线性量测函数，根据式（8）选定的状态向量和式（19）选定的量测向量，H(x)参照式（13）写出；G(x)为系统噪声输入函数，根据式（9）选定的系统噪声向量，G(x)参照式（1）写出；U(x)为量测噪声输入函数，根据式（20）选定的量测噪声向量，H(x)参照式（13）写出。

步骤3、将步骤2中建立的非线性连续系统模型离散化，形成非线性离散系统模型，非线性离散系统模型的状态方程与量测方程均为非线性；

离散化后，非线性离散系统模型为：

$\left\{\begin{array}{c}{x}_k=F\left(x_{k-1}\right)+w_{k-1}\\ z_k=H\left(x_k\right)+{\mathrm{\η}}_k\end{array}\right.$

式中，x_k和x_k-1分别为t_k和t_k-1时刻的系统状态向量；z_k为t_k时刻的量测向量；F(x_k-1)为t_k-1时刻的非线性状态转移函数，离散化时，采用四阶龙格库塔方法；H(x_k)为t_k时刻的非线性量测函数；w_k和w_k-1分别为t_k和t_k-1时刻的系统噪声向量；η_k为t_k时刻的量测噪声向量，且w_k和η_k满足：

$\left\{\begin{array}{c}E\left[w_k\right]=0,E\left[w_k{w}_j^T\right]=Q_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[{\mathrm{\η}}_k\right]=0,E\left[{\mathrm{\η}}_k{\mathrm{\η}}_j^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[w_k{\mathrm{\η}}_j^T\right]=0\end{array}\right.$

式中，δ_kj是δ函数；

离散系统噪声方差强度阵为：

Q_k＝G(x_k)QG(x_k)^TT_f

式中，Q_k为离散系统噪声w_k的方差强度阵；G(x_k)为t_k时刻的系统噪声输入函数；Q为连续系统噪声w的方差强度阵；T_f为滤波周期；

离散量测噪声方差强度阵为：

R_k＝U(x_k)RU(x_k)^T/T_f

式中，R_k为离散系统噪声η_k的方差强度阵；U(x_k)为t_k时刻的系统量测噪声输入函数；R为连续系统噪声η的方差强度阵。

步骤4、针对步骤3中建立的非线性离散系统模型，采用容积卡尔曼非线性滤波器进行滤波，滤波周期与惯性器件的数据更新周期一致，周期为T_f。

容积卡尔曼非线性滤波器的滤波步骤为:

1）初始化系统状态向量及其方差矩阵：

${\hat{x}}_0=E\left(x_0\right),P_0=E\left(({\hat{x}}_0-x_0){({\hat{x}}_0-x_0)}^T\right)$

式中，和P₀分别为t₀时刻的系统状态向量估计值及其方差阵；

2）时间更新：

分解前一步的方差阵：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T$

式中，P_k-1|k-1和S_k-1|k-1分别为t_k-1时刻的系统状态向量方差阵及其平方根阵；

计算状态容积点：

$X_{i,k|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}$

式中，X_i，k-1|k-1为t_k-1时刻的系统状态向量的第i个容积点；为t_k-1时刻的系统状态向量估计值；ξ_i为容积卡尔曼第i个容积点；

计算非线性状态函数传播的容积点：

$X_{i,k|k-1}^{*}=F\left(X_{i,k-1|k-1}\right)$

式中，为χ_i,k-1|k-1通过非线性状态函数F(χ_i,k-1|k-1)传播的容积点；

计算状态的一步预测值：

${\hat{x}}_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{X}_{i,k|k-1}^{*}{a}_i$

式中，为一步预测的t_k时刻系统状态向量;a_i为容积点ξ_i对应的权重；m为容积点总个数；i为容积点的序号；

计算一步预测方差阵：

$P_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{a}_i{X}_{i,k|k-1}^{*}{X}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{\mathrm{kk}-1}^T+Q_{k-1}$

式中，P_k|k-1为一步预测的t_k时刻系统状态向量方差阵;Q_k-1为t_k-1时刻的系统噪声方差强度阵；

3）量测更新：

分解一步预测方差阵：

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T$

式中，S_k|k-1为一步预测的t_k时刻系统状态向量方差阵的平方根阵；

计算一步预测容积点：

$X_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1}$

式中，χ_i,_k|k-1为一步预测的t_k时刻系统状态向量的第i个容积点；

计算通过非线性量测函数传播的量测容积点：

Z_i,k|k-1＝H(χ_i,k|k-1)

式中，Z_i,k|k-1为χ_i,k|k-1通过非线性量测函数H(χ_i,k|k-1)传播的容积点，即一步预测的t_k时刻系统量测向量的第i个容积点；

计算一步预测量测值：

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{Z}_{i,k|k-1}{a}_i$

式中，为一步预测的t_k时刻量测向量；

计算一步预测量测方差阵：

$P_{\mathrm{zz},\mathrm{ik}|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{a}_i{Z}_{i,k|k-1}{Z}_{i,\mathrm{kk}-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k$

式中，P_zz,k|k-1为一步预测的t_k时刻量测方差阵；

计算一步预测协方差阵：

$P_{\mathrm{xz},k|k-1}=\underset{i=1}{\overset{m}{\mathrm{\Σ}}}{a}_i{X}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T$

式中，P_xz,k|k-1为一步预测的t_k时刻协方差阵；

计算卡尔曼滤波增益：

$K_k=P_{\mathrm{xz},k|k-1}{P}_{\mathrm{zz},k|k-1}^{-1}$

式中，K_k为t_k时刻卡尔曼滤波增益阵；

更新状态：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

更新状态方差阵：

$P_{k|k}=P_{k|k-1}-K_k{P}_{\mathrm{zz},k|k-1}{K}_k^T$

上述算法中，容积卡尔曼容积点以及相应权重为

$\left\{\begin{array}{c}{\mathrm{\ξ}}_i=3\×[1{]}_1\\ a_i=\frac{1}{18}\end{array},i=1,\·\·\·,18\right.$

式中，

$[1{]}_i\∈\left\{\begin{array}{c}\left(\begin{array}{c}1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 1\\ .\\ .\\ .\\ 0\end{array}\right),...,\left(\begin{array}{c}-1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right),\left(\begin{array}{c}0\\ -1\\ .\\ .\\ .\\ 0\end{array}\right),...,\left(\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ -1\end{array}\right)\end{array}\right\},$

且每个列向量有9个元素，共有18个列向量；

容积点总数目为

m=18；

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (1)

1.基于重力加速度矢量匹配的非线性初始对准方法，包括下列步骤：

步骤1、捷联惯性导航系统粗对准：根据惯性器件陀螺仪测得的角速度信息和加速度计测得的线加速度信息，忽略惯性器件的测量误差，使用基于惯性系重力加速度的凝固解析粗对准算法求得粗略的初始纵摇角θ、横摇角γ和航向角ψ；

步骤2、在载体线速度为零的前提下，根据捷联惯性导航系统的欧拉角微分方程、惯性器件误差模型以及以导航坐标系重力加速度矢量为量测量的量测方程，选定9维系统状态量和3维量测量，建立基于重力加速度矢量匹配的非线性连续系统模型，其状态方程和量测方程均为非线性；

具体为：

以东北天地理坐标系作为导航坐标系，即n系，以载体右前上方向矢量右手定则构成的坐标系作为载体坐标系，即b系，假设载体线速度为零；

捷联惯性导航系统的欧拉角微分方程为：

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\γ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\frac{1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}& -\cos \mathrm{\γ}\sin \mathrm{\θ}\\ -\sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]{\mathrm{\ω}}_{nb}^b$

式中，θ为纵摇角；γ为横摇角；ψ为航向角；为载体坐标系相对导航坐标系的角速度矢量在载体坐标系轴向的分量构成的列矩阵，在载体线速度为零的情况下， 为载体坐标系轴向三只陀螺仪无误差的测量值， ${\mathrm{\ω}}_{ib}^b={\[{\widetilde{\mathrm{\ω}}}_{ibx}^b-{\mathrm{\δ\ω}}_{ibx}^b,{\widetilde{\mathrm{\ω}}}_{iby}^b-{\mathrm{\δ\ω}}_{iby}^b,{\widetilde{\mathrm{\ω}}}_{ibz}^b-{\mathrm{\δ\ω}}_{ibz}^b\]}^T,$ 和为载体坐标系轴向三只陀螺仪测量值，和为三只陀螺仪相应的测量误差， ${\mathrm{\δ\ω}}_{ibx}^b={\mathrm{\ϵ}}_{gx}+w_{gx},{\mathrm{\δ\ω}}_{iby}^b={\mathrm{\ϵ}}_{gy}+w_{gy},{\mathrm{\δ\ω}}_{ibz}^b={\mathrm{\ϵ}}_{gz}+w_{gz},$ ε_gx、ε_gy和ε_gz为三只陀螺仪测量误差的随机常值部分，w_gx、w_gy和w_gz为三只陀螺仪测量误差的随机噪声部分，随机噪声均假设为零均值高斯白噪声；为姿态矩阵， $C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\θ}\sin \mathrm{\γ}\sin \mathrm{\ψ}& \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\θ}\cos \mathrm{\γ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\θ}\cos \mathrm{\γ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right];$ 为地球自转角速度在导航坐标系中的投影，ω_ie为地球自转角速度，L为当地地理纬度；

捷联惯性导航系统的惯性器件测量误差的随机常值部分模型为：

${\stackrel{\·}{\▿}}_{ax}=0,{\stackrel{\·}{\▿}}_{ay}=0,{\stackrel{\·}{\▿}}_{az}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{gx}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{gy}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{gz}=0;$

选定的系统状态向量为：

$X={\[\mathrm{\θ},\mathrm{\γ},\mathrm{\ψ},{\▿}_{ax},{\▿}_{ay},{\▿}_{az},{\mathrm{\ϵ}}_{gx},{\mathrm{\ϵ}}_{gy},{\mathrm{\ϵ}}_{gz}\]}^T$

式中，x为系统状态向量，9维；

系统噪声向量为：

w＝[w_gx,w_gy,w_gz,0,0,0,0,0,0]^T

式中，w为系统噪声向量；

在载体线速度为零的情况下，以导航系重力加速度矢量为量测量的量测方程为：

gⁿ＝-fⁿ

式中，gⁿ为重力加速度矢量在n系中投影，其中gⁿ＝[0,0,-g]^T，g为重力加速度；fⁿ为加速度计无误差的测量值在n系中投影，其中 为载体系坐标轴方向三只加速度计无误差的测量值，其中 $f_{ib}^b={\[{\tilde{f}}_{ibx}^b-{\mathrm{\δf}}_{ibx}^b,{\tilde{f}}_{iby}^b-{\mathrm{\δf}}_{iby}^b,{\tilde{f}}_{ibz}^b-{\mathrm{\δf}}_{ibz}^b\]}^T,$ 和分别为三只加速度计相应的测量值；和分别为三只加速度计相应的测量误差，其中 ${\mathrm{\δf}}_{ibx}^b={\▿}_{ax}+w_{ax},{\mathrm{\δf}}_{iby}^b={\▿}_{ay}+w_{ay},{\mathrm{\δf}}_{ibz}^b={\▿}_{az}+w_{az},$ 和为三只加速度计测量误差的随机常值部分，w_ax、w_ay和w_az为三只加速度计测量误差的随机噪声部分，随机噪声均假设为零均值高斯白噪声；

选择gⁿ为量测量，则3维量测量为：

z＝[0,0,-g]^T

式中，z为量测向量；

量测噪声向量为：

η＝[w_ax,w_ay,w_az]^T

式中，η为量测噪声向量；

根据捷联惯性导航系统的欧拉角微分方程、惯性器件误差模型以及量测方程建立非线性滤波连续系统模型如下：

$\left\{\begin{array}{c}\stackrel{\·}{x}=F\left(x\right)+G\left(x\right)w\\ z=H\left(x\right)+U\left(x\right)\mathrm{\η}\end{array}\right.$

式中，F(x)为非线性状态转移函数，根据选定的状态向量参照欧拉角微分方程和惯性器件误差模型写出；H(x)为非线性量测函数，根据选定的状态向量和量测向量参照量测方程写出；G(x)为系统噪声输入函数，根据选定的状态向量和系统噪声向量参照欧拉角微分方程写出；U(x)为量测噪声输入函数，根据选定的量测向量和量测噪声向量参照量测方程写出；

步骤3、将步骤2中建立的非线性连续系统模型离散化，形成非线性离散系统模型，非线性离散系统模型的状态方程与量测方程均为非线性；

非线性离散系统模型为：

$\left\{\begin{array}{c}{x}_k=F\left(x_{k-1}\right)+w_{k-1}\\ z_k=H\left(x_k\right)+{\mathrm{\η}}_k\end{array}\right.$

式中，x_k和x_k-1分别为t_k和t_k-1时刻的系统状态向量；z_k为t_k时刻的量测向量；F(x_k-1)为t_k-1时刻的非线性状态转移函数，离散化时，采用四阶龙格库塔方法；H(x_k)为t_k时刻的非线性量测函数；w_k和w_k-1分别为t_k和t_k-1时刻的系统噪声向量；η_k为t_k时刻的量测噪声向量，且w_k和η_k满足：

$\left\{\begin{array}{c}E\[w_k\]=0,E\[w_k{w}_j^T\]=Q_k{\mathrm{\δ}}_{kj}\\ E\[{\mathrm{\η}}_k\]=0,E\[{\mathrm{\η}}_k{\mathrm{\η}}_j^T\]=R_k{\mathrm{\δ}}_{kj}\\ E\[w_k{\mathrm{\η}}_j^T\]=0\end{array}\right.$

式中，δ_kj是δ函数；

离散系统噪声方差强度阵为：

Q_k＝G(x_k)QG(x_k)^TT_f

式中，Q_k为离散系统噪声w_k的方差强度阵；G(x_k)为t_k时刻的系统噪声输入函数；Q为连续系统噪声w的方差强度阵；T_f为滤波周期；

离散量测噪声方差强度阵为：

R_k＝U(x_k)RU(x_k)^T/T_f

式中，R_k为离散系统噪声η_k的方差强度阵；U(x_k)为t_k时刻的系统量测噪声输入函数；R为连续系统噪声η的方差强度阵；

步骤4、针对步骤3中建立的非线性离散系统模型，采用容积卡尔曼非线性滤波器进行滤波，滤波周期与惯性器件的数据更新周期一致，周期为T_f；

其中容积卡尔曼非线性滤波器的滤波步骤为：

1)初始化系统状态向量及其方差矩阵：

${\hat{x}}_0=E\left(x_0\right),P_0=E\left(\right({\hat{x}}_0-x_0\left){\left({\hat{x}}_0-x_0\right)}^T\right)$

式中，和P₀分别为t₀时刻的系统状态向量估计值及其方差阵；

2)时间更新：

分解前一步的方差阵：

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T$

式中，P_k-1|k-1和S_k-1|k-1分别为t_k-1时刻的系统状态向量方差阵及其平方根阵；

计算状态容积点：

${\mathrm{\χ}}_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}$

式中，χ_i,k-1|k-1为t_k-1时刻的系统状态向量的第i个容积点；为t_k-1时刻的系统状态向量估计值；ξ_i为容积卡尔曼第i个容积点；

计算非线性状态函数传播的容积点：

${\mathrm{\χ}}_{i,k|k-1}^{*}=F\left({\mathrm{\χ}}_{i,k-1|k-1}\right)$

式中，为χ_i,k-1|k-1通过非线性状态函数F(χ_i,k-1|k-1)传播的容积点；

计算状态的一步预测值：

${\hat{x}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{\mathrm{\χ}}_{i,k|k-1}^{*}{a}_i$

式中，为一步预测的t_k时刻系统状态向量；a_i为容积点ξ_i对应的权重；m为容积点总个数；i为容积点的序号；

计算一步预测方差阵：

$P_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}^{*}{\mathrm{\χ}}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}$

式中，P_k|k-1为一步预测的t_k时刻系统状态向量方差阵；Q_k-1为t_k-1时刻的系统噪声方差强度阵；

3)量测更新：

分解一步预测方差阵：

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T$

式中，S_k|k-1为一步预测的t_k时刻系统状态向量方差阵的平方根阵；

计算一步预测容积点：

${\mathrm{\χ}}_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1}$

式中，χ_i,k|k-1为一步预测的t_k时刻系统状态向量的第i个容积点；

计算通过非线性量测函数传播的量测容积点：

Z_i,k|k-1＝H(χ_i,k|k-1)

式中，Z_i,k|k-1为χ_i,k|k-1通过非线性量测函数H(χ_i,k|k-1)传播的容积点，即一步预测的t_k时刻系统量测向量的第i个容积点；

计算一步预测量测值：

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{Z}_{i,k|k-1}{a}_i$

式中，为一步预测的t_k时刻量测向量；

计算一步预测量测方差阵：

$P_{zz,k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{Z}_{i,k|k-1}{Z}_{i,k|k-1}^T{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k$

式中，P_zz,k|k-1为一步预测的t_k时刻量测方差阵；

计算一步预测协方差阵：

$P_{xz,k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}{Z}_{i,k|k-1}^T{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T$

式中，P_xz,k|k-1为一步预测的t_k时刻协方差阵；

计算卡尔曼滤波增益：

$K_k=P_{xz,k|k-1}{P}_{zz,k|k-1}^{-1}$

式中，K_k为t_k时刻卡尔曼滤波增益阵；

更新状态：

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

更新状态方差阵：

$P_{k|k}=P_{k|k-1}-K_k{P}_{zz,k|k-1}{K}_k^T.$