CN104748747A - 一种适用于装甲车的定位定向装置及方法 - Google Patents

Abstract

本发明涉及一种适用于装甲车的定位定向装置及方法,包括台体、第一敏感元件、第二敏感元件、功能电路、基座及减振器，内台体为正六面体，以正六面体的一个顶点为原点O，建立三维坐标系，定义XY面为正面；基座外部设立六方向基准面；内台体的正面、底面、右侧面上均设置有第一安装腔，三个第一敏感元件分别安装在第一安装腔内；正面上设置有两个第二安装腔，右侧面上设置有一个第二安装腔，三个第二敏感元件分别安装在第二安装腔内；第一安装腔的深度大于所述第二安装腔，内台体的底面设置有第三安装腔，功能电路安装在第三安装腔内。本发明解决了现有装甲车定位定向装置导航精度不高的技术问题，具有定位精度高，隐蔽性强，抗干扰能力强等特点。

Description

一种适用于装甲车的定位定向装置及方法

技术领域

本发明涉及一种定位定向装置及发法，尤其涉及一种为装甲车及其它发射车实时提供高精度姿态位置等信息的定位定向装置与方法。 

背景技术

我军装甲兵器导航定位目前已经有所发展，但是停留在单轴或双轴航向保持产品基础上，这种产品存在一些问题，例如在发动机工作条件下的寻北精度不高，车体倾斜对寻北有影响，导航精度不高等。 

近年来新研发的基于内减振的定位定向装置体积和重量都较大。采用转台标定存在控制难度高、成本增加、人工翻转六面体困难，单轴转台台面和功放都要增加的问题。因而就需要新设计一种不需要六面体而进行标定的定位定向装置。 

发明内容

本发明目的是提供一种适用于装甲车的定位定向装置及方法，其解决了现有装甲车定位定向装置导航精度不高的技术问题，是一种自主式、全姿态导航定位系统，具有全天候，不依赖外部信号，定位精度高，隐蔽性强，抗干扰能力强等特点。 

本发明的技术解决方案是： 

一种适用于装甲车的定位定向装置，包括台体、三个第一敏感元件、三个第二敏感元件、功能电路，所述内台体为正六面体，以正六面体的一个顶点为原点O，建立三维坐标系，定义XY面为正面； 

其特殊之处在于： 

还包括基座及减振器； 

所述基座外部设立六方向基准面，所述基准面利用六面体标定原理设立； 

所述内台体设置在基座内，所述减振器共8个，其中4个位于Y向内台体和基座之间且分布在四角，另外4个位于-Y向内台体和基座之间且分布在四角； 

所述内台体的正面、底面、右侧面上均设置有第一安装腔，所述三个第一敏感元件分别安装在第一安装腔内； 

所述正面上设置有两个第二安装腔，所述右侧面上设置有一个第二安装腔，所述三个第二敏感元件分别安装在第二安装腔内； 

所述第一安装腔的深度大于所述第二安装腔， 

所述内台体的底面设置有第三安装腔，所述功能电路安装在第三安装腔内。 

上述第一敏感元件为激光陀螺，所述第二敏感元件为加速度计。 

每个方向的基准面由4个小基准面构成；所述4个小基准面位于该方向基座外侧面的四个角上。 

上述敏感元件组2位于内台体水平面的正中心处。 

一种适用于装甲车的定位定向方法，包括以下步骤： 

1】标定： 

1.1】将定位定向装置放置在平板上，预热10min后，对定位定向装置进行位置翻转并进行数据采集； 

1.2】一个位置的数据采集完毕后再进行下一个位置的翻转与数据采集，直至所有位置翻转及数据采集完毕为止； 

1.3】根据采集的惯性仪表各项误差参数，建立所有误差参数与系统导航误差之间的关系； 

2】抗扰动初始对准 

在发动机工作时，对陀螺和加速度计原始测量数据进行低通滤波；在人员上下时，对陀螺和加速度计原始测量数据进行卡尔曼滤波； 

3】补偿 

3.1】温度补偿 

3.1.1】建立惯性仪表输出的零位温度补偿模型： 

(1)首先根据温度和零位数据绘制温度与零位之间的关系曲线，根据关系曲线确定分段情况，使得每一段曲线可以用三次曲线较好地拟合； 

(2)在每一个数据段内将惯性仪表零位建立为三阶模型，利用最小二乘算 法，得到三阶模型的系数，并给出误差估计，从而完成每一个惯性仪表零位温补模型的建立； 

3.1.2】激励惯性仪表在装甲车不同温度条件下的测量误差： 

(1)操作转台，使得转台定位面法线转至与正北夹角为+45°位置，记录此时转台控制台的位置设定角度准确数值，该位置即为变温补偿试验方案的第一个位置，记为位置1； 

(2)在位置1，-40℃温度点下，定位定向装置保温4h以后，按要求对定位定向装置进行通电，预热10min后开始测试，在位置1静态测试5min后保存数据，操作转台，使定位定向装置平稳地绕其天轴转动180°，此位置为位置2； 

(3)转台停稳后，在位置2静态测试5min后保存数据，继续操作转台，依次在位置1和位置2静态测试5min，并将数据存入相应的文件中； 

(4)按照如下温度曲线：-40℃保温30min，-40℃到+50℃升温4h，+50℃保温4h，重复步骤(1)至(3)，直至获得温度-测量误差曲线；每个位置保存的测试数据不应少于5min；连续两组测试数据不能都是在同一个位置测得，且要求采集时间间隔不大于3min； 

3.1.3】根据发射车实时环境温度,根据步骤3.1.2获得的温度-测量误差曲线进行温度补偿； 

3.2】高度定位补偿 

3.2.1】设定中间状态时的炮弹数量和油料容积，进行里程计刻度因数和安装误差标定，制定以中间状态为基准，不同弹数和油料变化引起的俯仰角变化量表； 

3.2.2】采用查表方式对俯仰角变化量进行修正； 

4】根据滤波和补偿后的测量数据确定初始姿态，包括航向角、俯仰角和横滚角； 

5】定位定向装置与车载里程计组合实现航位解算，实时输出发射车的位置、速度、航向、姿态信息。 

上述惯性仪表零位温补模型如下： 

以加速度计X通道为例，建立X加速度计零位的温补模型如下： 

公式（1）中，TK_0x表示X加速度计零位，单位为^/s；T_ax表示X加速度计温度信息，单位为℃； 

以陀螺X通道为例，建立X陀螺零位的温补模型如下： 

公式（2）中，TD_0x表示X陀螺零位，单位为^/s；T_gx表示X陀螺温度信息，单位为℃。 

上述卡尔曼滤波计算方程包括滤波计算回路和增益计算回路： 

所述滤波计算回路依次包括： 

状态一步预测 

${\hat{X}}_{k/k-1}={\mathrm{\φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(1\right)$

以及，状态估计 

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(2\right)$

所述滤波增益回路依次包括： 

滤波增益 

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(3\right)$

以及，一步预测均方误差 

$P_{k/k-1}={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(4\right)$

以及，估计均方误差 

P_k=(I-K_kH_k)P_k/k-1     （5） 

所述卡尔曼滤波计算方程中的初始参数设置如下： 

Q_k=diag([(5.0e-6)² (5.0e-6)² (5.0e-6)² 0 0]^T)， 

R_k=diag([(5.0e-3)² (5.0e-3)²]^T)，X₀=[0 0 0 0 0]^T， 

P₀=1.0e+8*I。 

本发明所具有优点： 

1、本发明可以为装甲车实时提供高精度姿态位置信息，为车载武器提供初始姿态基准，具有定位精度高，初始对准抗扰动，启动预热时间短等特点。 

2、本发明定位定向装置由激光陀螺和石英加速度计及其相应电路构成，其内部采用内减振技术，有效隔离外部安装界面对激光陀螺的影响。 

3、本发明定位定向装置内置导航计算机，具有高速运算能力，能提供载车的加速度、速度、角速度、位置、航向、姿态、惯性高度等信息。 

4、本发明定位定向装置结构紧凑，安装方便，精度高；基座外部直接设立六方向基准面，通过机械加工保证各基准平面相互位置关系（相当于产品自带基准面），保证了定位定向装置可以实现自标定，改变了传统上六面体标定捷联惯组的传统，在保证产品的精度的同时降低了产品的成本。。 

5、本发明装置，具有内台体结构形式较为复杂，体积小；内台体结构的比刚度和动态特性高；基座中空设计，保证了定位定向装置内部走线方便。 

附图说明

图1是定位定向装置及减振器示意图；图中减振器共2×4个，用于±Y向上连接内台体和基座； 

图2和图3是内台体结构示意图； 

图4和图6是敏感元件在内台体结构中的安装位置示意图； 

图5是功能电路在内台体结构中的安装位置示意图； 

图7是基座结构及标定六面体结构示意图； 

图8是卡尔曼滤波的原理框图； 

图9温箱温度控制曲线； 

其中附图标记为：1-基座，2-内台体，3-减振器，4-加速度计，5-激光陀螺，6-功能电路，7-内台体正面，8-基准面。 

具体实施方式

1、内减振技术的定位定向装置 

定位定向装置在结构设计上采用“内台体”和“基座”方案，激光陀螺、加速度计以及激光陀螺高压控制电路安装在内台体上，对内台体结构进行优化设计，提高内台体表头安装面刚度，避开激光陀螺机抖谐振频率，从而抑制由台体结构引起的耦合响应。内台体与基座间通过三向橡胶阻尼减振器连接，隔离外部环境与敏感元件间的交联互扰。 

为了给激光陀螺和加速度计创造良好的工作环境条件，采用一体化内减振结构设计方案，通过三维减振器消除因载车振动和箱体安装方式给陀螺带来的影响。通过将加速度计配置在质心和减振中心附近消除减振器引入的抖动对加速度计输出精度带来不利影响。内减振技术的定位定向装置示意图如图1所示。 

本发明设计了一种新的基于内减振和自带外部标定基准的定位定向装置结构，结构主要由内台体和基座组成，在原来体积外形和质量不发生改变的情况下，尽量提高内台体结构的刚度，使内台体结构的固有频率避开了敏感元件的谐振频率及工作频率，从而为惯性仪表在各种复杂的动态力学环境下稳定工作提供了保证，满足敏感元件在工作时的动态特性要求。另外，利用六面体标定原理在基座外部直接设立六方向基准面，通过机械加工保证各基准平面相互位置关系，从而取消了六面体，在保证产品精度的同时降低了产品成本。具体如下： 

（1）在原来体积外形的基础上，将内台体结构设计成由多个复杂型腔组成的框架式结构，并利用内台体各型腔的侧壁对内台体薄弱环节进行加强，尽量提高内台体的刚度、强度和动态特性，避免因振动引起的扰动对敏感元件的干扰，见图2和图3所示。 

（2）利用内台体结构中的型腔安装敏感元件，增大内台体截面的惯性积和截面系数，保证内台体在X、Y、Z三个方向的动态特性保持一致，见图4所示。 

（3）利用内台体上所形成的型腔布置敏感元件所需的功能电路，并缩短了各电路板和敏感元件的走线距离，减小了因长距离走线带来的信号损失，有利于提高装置的输出精度，见图5所示。 

（4）内台体组件上所有零组件的分布和安装使得各零组件装拆方便，保 证在目前组成、不增加配重的前提下使内台体组件基本达到平衡。将敏感元件组2尽可能安装于内台体水平面（内台体+X方向面，即附图6中涂黑的面）的正中心处，可以减弱敏感元件组2在振动环境下的漂移对装置精度影响，见附图6所示。 

（5）定位定向装置基座在保证刚度、强度的基础上，采取中空设计保证了定位定向装置内部走线方便。基座外部通过机械加工直接设立六方向基准面，从而取消六面体，克服了大体积激光捷联惯导的标定困难，在保证产品的精度的同时降低了产品成本，见附图7所示。 

2、基于内减振的高精度激光捷联惯导标定技术 

为了克服内减振带来的标定困难，在箱体直接加工出标定六面体，消除二次安装带来的安装误差变化的不利影响。具体实现如下： 

a)将定位定向装置放置在平板上，预热10min后，对定位定向装置进行位置翻转并进行数据采集； 

b)一个位置的数据采集完毕后再进行下一个位置的翻转与数据采集，直至所有位置翻转及数据采集完毕为止。 

通过翻转的一系列位置充分激励惯性仪表各项误差参数，建立起所有误差参数与系统导航误差之间的关系，通过测量每个位置静态导航状态下的速度误差，采用最小二乘估计，全面辨识惯性仪表所有误差参数。此标定方法时间短、易操作、所需设备简易、所需人员少。 

图7中为定位定向装置标定六面体，六个面均有四个定位基准面，并且每个面的四个定位基准面在一个平面上，因此定位定向装置六个面的定位基面准构成一个标定六面体。 

3、抗扰动初始对准技术 

武器系统要求定位定向装置能够在发射车静止、发动机工作、人员上下、倾斜不大于10°、时间不大于5min等情况下确定初始姿态（包括航向角、俯仰角和横滚角），然后定位定向装置与车载里程计组合实现航位解算，实时输出发射车的位置、速度、航向、姿态等信息。 

抗扰动初始对准技术实现了在发动机工作和人员上下时的精确姿态确定。 在发动机工作时，定位定向装置存在高频振动（典型值为800Hz），需要对陀螺和加速度计原始测量数据进行低通滤波。由于定位定向装置中使用的是90型二频机抖激光陀螺，为消除锁区环形激光陀螺谐振腔随机械抖动机构做高频抖动（400Hz左右），为获得真实的角速率测量信息对陀螺输出数据进行了低通滤波（100Hz左右），通过该低通滤波器对陀螺和加速度计信号进行处理，能够同时消除发动机的高频振动和陀螺自身的高频抖动。在人员上下时，定位定向装置存在低频扰动，为此设计了卡尔曼滤波器，对低频变化信号进行实时跟踪。该初始姿态确定方法分为粗对准阶段和精对准阶段，通过合理分配粗、精对准时间，使得在粗对准阶段快速确定定位定向装置的粗略初始姿态，在精对准阶段精确确定初始姿态。在粗对准阶段，对初始姿态精度要求不高，但要求对准时间短，并达到一定的姿态精度；在精对准阶段，根据粗对准达到的精度及系统使用环境，合理设置卡尔曼滤波器的相关参数，以达到快速精确确定三个初始姿态角的目的。 

卡尔曼滤波计算方程如下： 

状态一步预测 

${\hat{X}}_{k/k-1}={\mathrm{\φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(1\right)$

状态估计 

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(2\right)$

滤波增益 

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(3\right)$

一步预测均方误差 

$P_{k/k-1}={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(4\right)$

估计均方误差 

P_k=(I-K_kH_k)P_k/k-1     （5） 

方程中的初始参数设置如下： 

Q_k=diag([(5.0e-6)² (5.0e-6)² (5.0e-6)² 0 0]^T)， 

R_k=diag([(5.0e-3)² (5.0e-3)²]^T)，X₀=[0 0 0 0 0]^T， 

P₀=1.0e+8*I。 

卡尔曼滤波原理框图如图8所示 

4、温度补偿技术 

为了满足10min预热要求，在硬件上取消了温度控制回路，通过对温度试验测试数据进行分析，建立加速度计和激光陀螺的温度模型并储存在计算机中，由软件进行实时补偿，通过温度补偿，保证全温段对准精度。 

具体实现如下： 

a.从系统的角度出发，将温度对系统内部惯性仪表、电子线路以及机械结构等的影响统一归结为对惯性仪表输出的影响，建立惯性仪表输出的温度补偿模型，每一个惯性仪表零位温补模型的建立过程如下： 

(1)首先根据温度和零位数据绘制温度与零位之间的关系曲线，根据温度——零位曲线确定分段情况，一般选取曲线的拐点作为分段点，使得每一段曲线可以用三次曲线较好地拟合。 

(2)在每一个数据段内将惯性仪表零位建立为三阶模型，利用最小二乘算法，得到三阶模型的系数，并给出误差估计，从而完成每一个惯性仪表零位温补模型的建立。 

利用以上过程，以加速度计X通道为例，建立X加速度计零位的温补模型如下： 

公式（1）中，TK_0x表示X加速度计零位，单位为^/s；T_ax表示X加速度计温度信息，单位为℃。 

以陀螺X通道为例，建立X陀螺零位的温补模型如下： 

公式（2）中，TD_0x表示X陀螺零位，单位为^/s；T_gx表示X陀螺温度信息，单 位为℃。 

b.结合发射车环境温度条件,采取温度建模实验方案，充分激励惯性仪表在该温度条件下的测量误差，本发明采用变温补偿试验方案，具体如下： 

1）按照图9的温度控制曲线设定温箱温度。操作转台，使得转台定位面法线转至与正北夹角为+45°位置(绕系统天轴正转45°位置)，记录此时转台控制台的位置设定角度准确数值，该位置即为变温补偿试验方案的第一个位置，记为位置1。 

在位置1，-40℃温度点下，定位定向装置保温4h以后，按要求对定位定向装置进行通电，预热10min后开始测试，在位置1静态测试5min后保存数据，操作转台，使定位定向装置平稳地绕其天轴转动180°，此位置为位置2，转台停稳后，在位置2静态测试5min后保存数据，继续操作转台，依次在位置1和位置2静态测试5min,并将数据存入相应的文件中，直到图9中的温度控制曲线结束。 

在整个测试过程中，温度变化可以描述为：-40℃保温30min，-40℃到+50℃升温4h，+50℃保温4h，并且转台必须间隔设定在位置1和位置2。每个位置保存的测试数据不应少于5min。在-40℃保温30min状态下，测试得到的数据文件必须是成对的（数据文件个数是偶数），连续两组测试数据不能都是在同一个位置测得，且要求采集时间间隔不大于3min。 

该温度补偿技术可以有效减小环境温度变化带来的惯性仪表测量误差，提高系统的精度和快速反应能力，实现简单方便，时间较短，适用于各种车载环境条件下的中高精度激光陀螺捷联惯导系统。 

5、高度定位补偿技术 

载弹量和油量变化会引起发射车俯仰角变化，该变化值会引起里程计刻度系数的变化，进而引起里程计与定位定向装置的组合导航精度超差，为此，以4发弹、550L油为中间状态，进行里程计刻度因数和安装误差标定，制定了以中间状态为基准，不同弹数和油料变化引起的俯仰角变化量表。采用查表方式对俯仰角变化量进行修正，很好地解决了不同载弹量和油量对组合导航精度的影响问题。 

Claims (7)

1.一种适用于装甲车的定位定向装置，包括台体、三个第一敏感元件、三个第二敏感元件、功能电路，所述内台体为正六面体，以正六面体的一个顶点为原点O，建立三维坐标系，定义XY面为正面，与之相对的面为底面；

其特征在于：

还包括基座及减振器；

所述基座外部设立六方向基准面，所述基准面利用六面体标定原理设立；

所述内台体设置在基座内，所述减振器共8个，其中4个位于Y向内台体和基座之间且分布在四角，另外4个位于-Y向内台体和基座之间且分布在四角；

所述内台体的正面、底面、右侧面上均设置有第一安装腔，三个第一敏感元件分别安装在第一安装腔内；

所述正面上设置有两个第二安装腔，所述右侧面上设置有一个第二安装腔，所述三个第二敏感元件分别安装在第二安装腔内；

所述第一安装腔的深度大于所述第二安装腔，

所述内台体的底面设置有第三安装腔，所述功能电路安装在第三安装腔内。

2.根据权利要求1所述的适用于装甲车的定位定向装置，其特征在于：所述第一敏感元件为激光陀螺，所述第二敏感元件为加速度计。

3.根据权利要求1或2所述的适用于装甲车的定位定向装置，其特征在于：每个方向的基准面由4个小基准面构成；所述4个小基准面位于该方向基座外侧面的四个角上。

4.根据权利要求1或2所述的适用于装甲车的定位定向装置，其特征在于：所述敏感元件组2位于内台体水平面的正中心处。

5.一种适用于装甲车的定位定向方法，其特征在于：包括以下步骤：

1】标定：

1.1】将定位定向装置放置在平板上，预热10min后，对定位定向装置进行位置翻转并进行数据采集；

1.2】一个位置的数据采集完毕后再进行下一个位置的翻转与数据采集，直至所有位置翻转及数据采集完毕为止；

1.3】根据采集的惯性仪表各项误差参数，建立所有误差参数与系统导航误差之间的关系；

2】抗扰动初始对准

在发动机工作时，对陀螺和加速度计原始测量数据进行低通滤波；在人员上下时，对陀螺和加速度计原始测量数据进行卡尔曼滤波；

3】补偿

3.1】温度补偿

3.1.1】建立惯性仪表输出的零位温度补偿模型：

(1)首先根据温度和零位数据绘制温度与零位之间的关系曲线，根据关系曲线确定分段情况，使得每一段曲线可以用三次曲线较好地拟合；

(2)在每一个数据段内将惯性仪表零位建立为三阶模型，利用最小二乘算法，得到三阶模型的系数，并给出误差估计，从而完成每一个惯性仪表零位温补模型的建立；

3.1.2】激励惯性仪表在装甲车不同温度条件下的测量误差：

(1)操作转台，使得转台定位面法线转至与正北夹角为+45°位置，记录此时转台控制台的位置设定角度准确数值，该位置即为变温补偿试验方案的第一个位置，记为位置1；

(2)在位置1，-40℃温度点下，定位定向装置保温4h以后，按要求对定位定向装置进行通电，预热10min后开始测试，在位置1静态测试5min后保存数据，操作转台，使定位定向装置平稳地绕其天轴转动180°，此位置为位置2；

(3)转台停稳后，在位置2静态测试5min后保存数据，继续操作转台，依次在位置1和位置2静态测试5min，并将数据存入相应的文件中；

(4)按照如下温度曲线：-40℃保温30min，-40℃到+50℃升温4h，+50℃保温4h，重复步骤(1)至(3)，直至获得温度-测量误差曲线；每个位置保存的测试数据不应少于5min；连续两组测试数据不能都是在同一个位置测得，且要求采集时间间隔不大于3min；

3.1.3】根据发射车实时环境温度,根据步骤3.1.2获得的温度-测量误差曲线进行温度补偿；

3.2】高度定位补偿

3.2.1】设定中间状态时的炮弹数量和油料容积，进行里程计刻度因数和安装误差标定，制定以中间状态为基准，不同弹数和油料变化引起的俯仰角变化量表；

3.2.2】采用查表方式对俯仰角变化量进行修正；

4】根据滤波和补偿后的测量数据确定初始姿态，包括航向角、俯仰角和横滚角；

5】定位定向装置与车载里程计组合实现航位解算，实时输出发射车的位置、速度、航向、姿态信息。

6.根据权利要求5所述的适用于装甲车的定位定向方法，其特征在于：

所述惯性仪表零位温补模型如下：

以加速度计X通道为例，建立X加速度计零位的温补模型如下：

公式（1）中，TK_0x表示X加速度计零位，单位为^/s；T_ax表示X加速度计温度信息，单位为℃；

以陀螺X通道为例，建立X陀螺零位的温补模型如下：

公式（2）中，TD_0x表示X陀螺零位，单位为^/s；T_gx表示X陀螺温度信息，单位为℃。

7.根据权利要求5所述的适用于装甲车的定位定向方法，其特征在于：

所述卡尔曼滤波计算方程包括滤波计算回路和增益计算回路：

所述滤波计算回路依次包括：

状态一步预测

${\hat{X}}_{k/k-1}={\mathrm{\φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(1\right)$

以及，状态估计

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(2\right)$

所述滤波增益回路依次包括：

滤波增益

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(3\right)$

以及，一步预测均方误差

$P_{k/k-1}={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(4\right)$

以及，估计均方误差

P_k=(I-K_kH_k)P_k/k-1     （5）

所述卡尔曼滤波计算方程中的初始参数设置如下：

Q_k=diag([(5.0e-6)² (5.0e-6)² (5.0e-6)² 0 0]^T)，

R_k=diag([(5.0e-3)² (5.0e-3)²]^T)，X₀=[0 0 0 0 0]^T，

P₀=1.0e+8*I。