CN115265473A - 一种基于ipv6的高度表与加速度计混合空间测高方法 - Google Patents

Abstract

本发明是关于一种基于IPV6的高度表与加速度计混合空间测高方法。其首先安装SiA200 MEMS加速度传感器，测量载体加速度；FYC‑LG24‑800型无线电高度表，测量载体高度信息。将上述高度与加速度信息由IPV6网络发送到地面站汇总并进行两次积分得到惯性速度信号与惯性高度信号；然后对载体高度信息进行非线性低通滤波，得到低通滤波高度信号并得到修正惯性高度误差信号，叠加惯性速度信号与积分得到一次修正惯性速度信号与两次修正后的惯性高度信号，并与高度表经非线性低通滤波后的信号组合卡尔曼滤波得到组合后的高度信号，从而形成混合测高信号，从而比单独测量具有更佳的效果。

Description

一种基于IPV6的高度表与加速度计混合空间测高方法

技术领域

本发明涉及车辆，运动体以及小微型无人飞行器的高精度高度测量领域，具体而言，涉及一种基于IPV6的高度表与加速度计混合空间测高的设计方法。

背景技术

精确的高度信息测量获取对无人飞行器以及飞艇、汽车等在导航与运动控制领域均至关重要。目前高度测量中，无线电高度表作为一种通过反射面来测高的装置，可以在各种气候条件下测量飞行器距地面的相对高度，是很多飞行器里必备的一种导航设备之一。然而无线电高度表的测高范围通常在-20～2500英尺，因此测量范围有限，而且在低空起飞进近条件下使用时，使用环境恶劣，雷达天线易产生故障从而影响测高的准确性。而在惯组单元中，通过垂直方向的加速度计与陀螺仪可以测量飞行器的相对高度，此种方法具有较好的可靠性，只是采用加速度计进行高度测量时，由于加速度到高度需要经过两次积分，积分会导致累积误差随着时间的增加而越来越大。因此如何将这两种测量高度的方式进行融合，取长补短，得到最终的多信息融合的组合测量方法，是一项非常具有实际意义的工程技术研究。同时，对于某些小微型飞行器或者飞行器集群，不便于在载体上进行数据融合以及卡尔曼滤波等复杂算法，需要将数据传递到地面站进行统一处理。基于上述背景技术，本发明提出了一种基于IPV6网络，无线电高度表与加速度计信息卡尔曼滤波融合的测高方法，该方法可以具有很高的精度和较高的工程应用价值。

需要说明的是，在上述背景技术部分发明的信息仅用于加强对本发明的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。

发明内容

本发明的目的在于提供一种基于IPV6的高度表与加速度计混合空间测高方法，进而至少在一定程度上克服由于相关技术的限制和缺陷而导致的单一高度测量精度不高的问题。

根据本发明的一个方面，提供一种基于IPV6的高度表与加速度计混合空间测高方法，包括以下步骤：

步骤S10，在载体上安装SiA200 MEMS加速度传感器，在载体上安装FYC-LG24-800型无线电高度表，获取实时高度信息，并通过IPV6网络将加速度计测量的载体加速度信号与无线电高度表获取的实时高度信息传递给地面综合站，并对载体加速度信号进行积分得到惯性速度信号，对惯性速度信号进行积分得到惯性高度信号；

步骤S20，针对所述的无线电高度表测量得到的实时高度信号数据，设计非线性低通滤波器，得到无线电低通高度信号；

步骤S30，对得到的非线性低通滤波后的高度信号，与惯性高度两次修正后的信号相比较，得到修正的低通高度误差信号，对所得的修正的低通高度误差信号进行非线性变换，得到变换后的修正低通高度误差信号；

步骤S40，将惯性高度两次修正后的信号，与高度表测得的高度信号相比较，得到修正惯性高度误差信号，叠加该修正惯性高度误差信号至惯性速度信号，形成惯性速度的一次修正信号；

步骤S50，将得到的惯性速度一次修正信号，和修正惯性高度误差信号，以及变换后的修正低通高度误差信号，一起叠加至惯性高度两次修正后的信号在上一时刻的值，形成惯性高度两次修正后的信号在当前时刻的值，并将该值作为新的惯性高度两次修正后的值，进行迭代；

步骤S60，对得到的两次修正后的惯性高度信号，以及高度表经非线性低通滤波后的信号，进行组合卡尔曼滤波，得到组合后的高度信号最优值。

在本发明的一种示例实施例中，在载体上安装SiA200 MEMS加速度传感器，在载体上安装FYC-LG24-800型无线电高度表，获取实时高度信息，并通过IPV6网络将加速度计测量的载体加速度信号与无线电高度表获取的实时高度信息传递给地面综合站，并对载体加速度信号进行积分得到惯性速度信号，对惯性速度信号进行积分得到惯性高度信号，具体包括：

v＝∫adt；

h＝∫vdt；

其中a(n)对应的是时间t＝n*ΔT时刻采用SiA200 MEMS加速度传感器测量的载体加速度信号，简写为a，其中ΔT为数据的输出采样周期，dt表示对时间信号的积分，v为惯性速度信号，h为惯性高度信号；将FYC-LG24-800型无线电高度表测量得到的实时高度信号记作y₁，并将高度数据进行离散化，得到数据y₁(n)，其中n为数据采样点数，n＝1,2,3…，对应的是时间t＝n*ΔT时刻的无线电高度测量数据；ΔT为数据的输出采样周期。

在本发明的一种示例实施例中，对无线电高度信号进行非线性低通滤波，得到无线电低通高度信号，记作h_a(n)。具体包括；

e_w(n)＝y₁(n)-h_a(n)；

其中，e_w(n)为高度表误差信号；

其中，k₃、k₄、

为滤波器内部参数；Δh_a(n)为e_w(n)经公式计算得到无线电低通高度导数信号，在n＞1时刻，将该信号积分即可得到h_a(n)，如下所示；

其中，h_a(1)为h_a(n)信号在一时刻的采样值，该值在程序中需要先给出初值。

在本发明的一种示例实施例中，对上述得到的非线性低通滤波后的高度信号h_a(n)，与惯性高度两次修正后的信号相比较，得到修正的低通高度误差信号，记作

其中，惯性高度两次修正后的信号记作s(n)，其具体计算公式见下述内容；然后对

进行非线性变换，得到变换后的修正低通高度误差信号，记作

具体包括：

其中

为非线性变换参数。

在本发明的一种示例实施例中，将惯性高度两次修正后的信号s(n)与前述的高度表测得的高度信号y₁(n)相比较，得到修正惯性高度误差信号，记作e(n)，叠加e(n)至惯性速度信号，形成惯性速度的一次修正信号，记作v_r(n)。具体包括：

e(n)＝s(n)-y₁(n)；

v_r(n)＝v(n-1)+a(n)·ΔT+k₂·e(n)；

其中，k₂为e(n)叠加至v(n-1)的权重。

在本发明的一种示例实施例中，将得到的惯性速度一次修正信号v_r(n)、修正惯性高度误差信号e(n)、变换后的修正低通高度误差信号

一起叠加至惯性高度两次修正后的信号s(n)在上一时刻的值s(n-1)，从而得到惯性高度两次修正后的信号在当前时刻的值。具体包括：

其中，k₁为e(n)叠加至s(n-1)的权重；k₁₂为

叠加至s(n-1)的权重；s(1)为s(n)在第一个采样时刻的值，是需要在程序中设置的初值。

在本发明的一种示例实施例中，对得到的两次修正后的惯性高度信号s(n)，将其作为卡尔曼滤波器的状态预测值，对前述得到的高度表低通滤波信号h_a(n)，将其作为卡尔曼滤波器的观测值。二者进行组合卡尔曼滤波，得到组合后的高度信号最优值，记作h_k(n)。具体包括：

将卡尔曼滤波器中的误差协方差信号记作pp(n)，其计算公式如下，

其中，即(1)为pp(n)在第一个采样点的值，该值在程序中需要赋以初值；bb(n)为卡尔曼增益，pp₁(n)为误差协方差的预测值，这两个信号的计算公式如下，

pp₁(n)＝pp(n-1)+Q_ww；

bb(n)＝pp₁(n)·[pp₁(n)+R_ww]^-1

其中，Q_ww为设置的预测模型本身的噪声，R_ww为设置的测量噪声。

则最优值h_k(n)的计算公式为

h_k(n)＝s(n)+bb(n)·[h_a(n)-s(n)]。

该地面站获得的组合后的高度信号最优值具有良好的精度特性，同时又避免惯性测量的高度易产生发散的缺点。

有益效果

本发明提供的一种基于IPV6的高度表与加速度计混合空间测高方法，其通过多重高度误差反馈与速度误差反馈与积分以及卡尔曼滤波的方式，使得混合高度测量能够滤除无线电高度表测量的大量无规则随机误差，同时又能消除惯性加速度计积分带来的时间累积误差，从而使得两者高度测量的优势能够相互结合互补，实现更佳的混合测量效果。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本发明。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明提供的一种基于IPV6的高度表与加速度计混合空间测高方法的流程图；

图2是本发明实施例所提供方法的SiA200 MEMS加速度传感器实物图；

图3是本发明实施例所提供方法的FYC-LG24-800型无线电高度表实物图；

图4是本发明实施例所研究对象的惯导所测速度信号曲线(单位：米/秒)；

图5是本发明实施例所提供方法的惯导所测高度信号曲线(单位：米)；

图6是本发明实施例所提供方法的无线电高度表所测量的高度信号曲线(单位：米)；

图7是本发明实施例所提供方法的无线电高度表经非线性滤波后的高度信号曲线(单位：米)；

图8是本发明实施例所提供方法的变换后的修正低通高度误差信号曲线信号曲线；

图9是本发明实施例所提供方法的惯性速度的一次修正信号曲线(单位：米/秒)；

图10是本发明实施例所提供方法的惯导所测高度经两次修正后的高度信号曲线(单位：米)；

图11是本发明实施例所提供方法的组合卡尔曼滤波后的高度信号曲线(单位：米)；

图12是本发明实施例所提供方法的组合卡尔曼滤波中的误差协方差曲线；

图13是本发明实施例所提供方法的组合卡尔曼滤波中的误差协方差的预测信号曲线；

图14是本发明实施例所提供方法的组合卡尔曼滤波中的卡尔曼增益曲线。

具体实施方式

现在将在参考附图基础上更全面地描述示例实施方式。然而，示例实施方式能够以多种形式实施，且不应被理解为限于在此阐述的范例；相反，提供这些实施方式使得本发明将更加全面和完整，并将示例实施方式的构思全面地传达给本领域的技术人员。所描述的特征、结构或特性可以以任何合适的方式结合在一个或更多实施方式中。在下面的描述中，提供许多具体细节从而给出对本发明的实施方式的充分理解。然而，本领域技术人员将意识到，可以实践本发明的技术方案而省略所述特定细节中的一个或更多，或者可以采用其它的方法、组元、装置、步骤等。在其它情况下，不详细示出或描述公知技术方案以避免喧宾夺主而使得本发明的各方面变得模糊。

本发明提供了一种基于IPV6的高度表与加速度计混合空间测高方法，其通过安装SiA200 MEMS加速度传感器测量载体加速度进行两次积分得到惯性高度，然后通过无线电高度表测量得到载体的无线电高度信号，将无线电高度表测得的高度信号经非线性低通滤波后，与惯性高度经两次修正后的信号进行比较得到高度误差信号，并使用该信号对惯性速度和惯性误差进行迭代修正，并用修正后的惯性高度和滤波后的无线电高度进行卡尔曼组合滤波，得到最终的精度合成高度信号，从而使得整个混合高度测量方法具有很高的工程应用价值。

下面，将结合附图对本发明的一种基于IPV6的高度表与加速度计混合空间测高方法进行进一步的解释以及说明。参考图1所示，该一种基于IPV6的高度表与加速度计混合空间测高方法包括以下步骤：

步骤S10，在载体上安装SiA200 MEMS加速度传感器，在载体上安装FYC-LG24-800型无线电高度表，获取实时高度信息，并通过IPV6网络将加速度计测量的载体加速度信号与无线电高度表获取的实时高度信息传递给地面综合站，并对载体加速度信号进行积分得到惯性速度信号，对惯性速度信号进行积分得到惯性高度信号；

具体的，首先，在载体上安装SiA200 MEMS加速度传感器，其物理尺寸为小封装9*9nm2，其非常小，其物理尺寸如图2所示。功耗小于10mW，工作温度范围为-40度至+125度。加速度量程2g至30g可选，测量分辨率可达到0.00005g。将其安装在载体上，测量载体垂直方向的加速度信号，计作a(n)。其中n＝1,2,3…，对应的是时间t＝n*ΔT时刻的加速度信号，其中ΔT为数据的输出采样周期，然后将加速度信号a(n)通过IPV6网络传递给地面综合站。

其次，在地面综合站对载体加速度测量数据进行积分，得到惯性速度信号，记作v(n)，然后对速度信号进一步积分，得到惯性高度信号，记作h(n)，其积分过程如下所示：

v＝∫adt；

h＝∫vdt；

其中dt表示对时间信号积分。

最后，在载体上安装FYC-LG24-800型无线电高度表，其物理尺寸为120×66×27mm，重量小于400g，如图3所示，数据更新率为20Hz。测量载体高度，记作y₁，然后按照加速度计的数据间隔周期ΔT，将高度数据进行离散化，得到数据y₁(n)，其中n＝1,2,3…，对应的是时间t＝n*ΔT时刻的无线电高度测量数据，其中ΔT为数据的输出采样周期，并采用IPV6网络将无线电高度表获取的实时高度信息y₁(n)传递给地面综合站；

步骤S20，针对所述的无线电高度表测量得到的实时高度信号数据数据，设计非线性低通滤波器，得到无线电低通高度信号；

首先将经非线性低通滤波后的高度信号记为h_a(n)，其中n＝1,2,3…，对应的是时间t＝n*ΔT时刻的滤波后的无线电高度测量数据，其中ΔT为数据的输出采样周期。

然后将无线电高度表测得的信号y₁(n)与无线电低通高度信号h_a(n)作比较，得到高度表误差信号e_w(n)，公式如下，其中h_a(n)的求解公式见下述。

e_w(n)＝y₁(n)-h_a(n)

其次将高度表误差信号e_w(n)经滤波器计算得到无线电低通高度导数信号，记作Δh_a(n)，公式如下。

其中，k₃、k₄、

为滤波器内部参数。

最后将Δh_a(n)积分得到非线性低通滤波后的高度信号h_a(n)，公式如下。

其中，h_a(1)为h_a(n)信号在一时刻的采样值，该值是h_a(n)的初值，在程序中需要先给出其具体值。

步骤S30，对得到的非线性低通滤波后的高度信号，与惯性高度两次修正后的信号相比较，得到修正的低通高度误差信号，对所得的修正的低通高度误差信号进行非线性变换，得到变换后的修正低通高度误差信号；

首先将惯性高度两次修正后的信号记作s(n)，该信号的求解见下述步骤。其次，将两次修正后的惯性高度信号s(n)，与高度表非线性低通滤波后的信号h_a(n)作比较，得到修正低通高度误差信号，记作

公式如下；

最后，对所得的修正低通高度误差信号进行非线性变换，得到变换后的修正低通高度误差信号，记作

公式如下；

其中

为非线性变换参数。

步骤S40，将惯性高度两次修正后的信号，与高度表测得的高度信号相比较，得到修正惯性高度误差信号，叠加该修正惯性高度误差信号至惯性速度信号，形成惯性速度的一次修正信号；

首先将惯性高度两次修正后的信号s(n)，与高度表经离散化后的信号y₁(n)相比较，得到修正惯性高度误差信号，记作e(n)，公式如下；

e(n)＝s(n)-y₁(n)；

其次，叠加e(n)、以及惯性加速度信号与采样周期的乘积至惯性速度信号在上一时刻的值v(n-1)，得到惯性速度的一次修正信号，记作v_r(n)，公式如下；

v_r(n)＝v(n-1)+a(n)·ΔT+k₂·e(n)；

其中，a(n)为惯性加速度信号，v(n)为惯性速度信号，k₂为e(n)叠加至v(n-1)的权重。

步骤S50，将得到的惯性速度一次修正信号，和修正惯性高度误差信号，以及变换后的修正低通高度误差信号，一起叠加至惯性高度两次修正后的信号在上一时刻的值，形成惯性高度两次修正后的信号在当前时刻的值，并将该值作为新的惯性高度两次修正后的值，进行迭代；

将前述得到的修正惯性高度误差信号e(n)、与变换后的修正低通高度误差信号

以及惯性速度的一次修正信号v_r(n)与采样周期的乘积，一起叠加至惯性高度两次修正后的信号在上一时刻的值s(n-1)，得到新的惯性高度两次修正后的信号s(n)，公式如下；

其中，k₁为e(n)叠加至s(n-1)的权重；k₁₂为

叠加至s(n-1)的权重；s(1)为s(n)在第一个采样时刻的值，该值为s(n)的初值，需要在程序中先行设置，形成迭代后，以当前时刻新算出的值代替上一个的值，完成程序运算。

步骤S60，对得到的两次修正后的惯性高度信号，以及高度表经非线性低通滤波后的信号，进行组合卡尔曼滤波，得到组合后的高度信号最优值。

首先，将误差协方差信号记作pp(n)，并设置其在第一个采样点的值pp(1)，并将卡尔曼增益与误差协方差的预测值相乘，并与误差协方差的预测值相比较，得到n＞1时的误差协方差信号，如下式所示。其中卡尔曼增益记作bb(n)，误差协方差的预测值记作pp₁(n)，其计算公式见下文所述。

之后，设置预测模型本身的噪声，记作Q_ww，并叠加前述得到的误差协方差信号，得到误差协方差的预测值pp₁(n)，公式如下。

pp₁(n)＝pp(n-1)+Q_ww

其次，设置测量噪声，记作R_ww，并叠加上述得到的误差协方差的预测值pp₁(n)，对叠加后的信号取逆，并乘以误差协方差的预测值pp₁(n)，得到卡尔曼增益bb(n)，公式如下。

bb(n)＝pp₁(n)·[pp₁(n)+R_ww]^-1

最后，将前述得到的高度表低通滤波信号h_a(n)作为卡尔曼滤波器的观测值，将两次修正后的惯性高度信号s(n)作为卡尔曼滤波器的状态预测值，二者经比较后，用比较后的信号乘以卡尔曼增益bb)n)，再叠加上s(n)，得到经卡尔曼滤波后的状态最优值，记作h_k(n)，公式如下。

h_k(n)＝s(n)+bb(n)·[h_a(n)s(n)]

至此，最终求得的h_k(n)即作为整个高度表测量高度与惯性测量高度的综合高度输出，其具有良好的精度特性，同时又避免惯性测量的高度易产生发散的缺点。

案例实施与计算机仿真模拟结果分析

在步骤S10中，设置ΔT＝0.01，在载体上安装SiA200 MEMS加速度传感器，测量载体的加速度信号，进行积分得到载体的惯性速度信号如图4所示，再积分得到载体的惯性高度信号如图5所示，采用无线电高度表测量得到载体的高度如图6所示。

在步骤S20中，设置h_a(1)＝0，

k₃＝0.1，k₄＝1，得到无线电高度表非线性低通滤波后的信号曲线如图7所示。

在步骤S30中，设置

得到变换后的修正低通高度误差信号曲线如图8所示。

步骤S40中，设置k₂＝-0.01，得到惯性速度的一次修正信号曲线如图9所示。

步骤S50中，设置s(1)＝0，k₁＝-0.02，k₁₂＝-0.002，得到最终的两次修正后的惯性高度信号曲线如图10所示。

步骤S60中，设置pp(1)＝0.1，Q_WW＝0.3，R_WW＝0.4，得到高度表滤波高度与两次修正后的惯性高度的综合高度输出，即组合卡尔曼滤波后的高度信号最优值h_k(n)，如图11所示；以及误差协方差pp(n)如图12所示，误差协方差的预测值pp₁(n)如图13所示，卡尔曼增益bb(n)如图14所示。

由图4和图5可以看出，使用惯性组件进行高度测量时，随着时间的推移，其积分的累积误差越来越大，300秒时零高度的误差已经达到8米；而通过图6可以看出，采用无线电高度进行测高时，测量过程充满了噪声信号，幅值在±2米；在图10中，对惯性高度进行两次修正后，其高度维持在零高度附近±0.2米；在图7中，对无线电高度表进行非线性低通滤波后，其高度也维持在零高度附近±0.1米；对两次修正后的惯性高度和非线性低通滤波后的高度进行组合卡尔曼滤波后，其曲线如图11所示，其高度维持在零高度附近±0.1米，可见本发明所提供的组合高度滤波方法，大大地改善了单独高度表与惯性组件单独测量的精度，具有很高的工程实用价值。

本领域技术人员在考虑说明书及实践这类的发明后，将容易想到本发明的其他实施例。本申请旨在涵盖本发明的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本发明的一般性原理并包括本发明未指明的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本发明的真正范围和精神由权利要求指出。

Claims (1)

1.一种基于IPV6的高度表与加速度计混合空间测高方法，其特征在于，包括以下步骤：

步骤S10，在载体上安装SiA200 MEMS加速度传感器，在载体上安装FYC-LG24-800型无线电高度表，获取实时高度信息，并通过IPV6网络将加速度计测量的载体加速度信号与无线电高度表获取的实时高度信息传递给地面综合站，并对载体加速度信号进行积分得到惯性速度信号，对惯性速度信号进行积分得到惯性高度信号如下：

v＝∫adt；

h＝∫vdt；

其中a(n)对应的是时间t＝n*ΔT时刻采用SiA200 MEMS加速度传感器测量的载体加速度信号，简写为a，其中ΔT为数据的输出采样周期，dt表示对时间信号的积分，v为惯性速度信号，h为惯性高度信号；将FYC-LG24-800型无线电高度表测量得到的实时高度信号记作y₁；

步骤S20，对上述高度表测量得的实时高度信号进行非线性低通滤波，得到无线电低通滤波高度信号如下：

e_w(n)＝y₁(n)-h_a(n)；

其中y₁为安装FYC-LG24-800型无线电高度表测量得到的实时高度信号，并将高度数据进行离散化，得到数据y₁(n)，其中n＝1，2，3…，对应的是时间t＝n*ΔT时刻的无线电高度测量得到的实时高度信号数据，其中ΔT为数据的输出采样周期；h_a(n)为无线电低通滤波高度信号，h_a(1)为h_a(n)信号在一时刻的采样值，e_w(n)为高度表误差信号；Δh_a(n-1)为高度非线性微分信号；k₃、k₄、

为滤波器内部参数，为常值；

步骤S30，对所述的无线电低通滤波高度信号，与惯性高度两次修正后信号相比较，得到修正的低通高度误差信号，对所得的修正的低通高度误差信号进行非线性变换，得到变换后的修正低通高度误差信号：

其中h_a(n)为非线性低通滤波后的高度信号，s(n)为惯性高度两次修正后信号，其初始值选取为0；

为修正的低通高度误差信号，

为变换后的修正低通高度误差信号；

为非线性变换参数，为常值；

步骤S40，将惯性高度两次修正后信号，与高度表测得的高度信号相比较，得到修正惯性高度误差信号，叠加该修正惯性高度误差信号至惯性速度信号，形成惯性速度一次修正信号如下：

e(n)＝s(n)-y₁(n)；

v_r(n)＝v(n-1)+a(n)·ΔT+k₂·e(n)；

其中e(n)为修正惯性高度误差信号，v_r(n)为惯性速度一次修正信号，k₂为e(n)叠加至v(n-1)的权重；v(n-1)为惯性速度信号在t＝(n-1)*ΔT时刻的数据。

步骤S50，将所述的惯性速度一次修正信号和修正惯性高度误差信号，以及变换后的修正低通高度误差信号，一起叠加至惯性高度两次修正后的信号在上一时刻的值，形成惯性高度两次修正后的信号在当前时刻的值，并将该值作为新的惯性高度两次修正后的值，进行迭代计算如下：

其中k₁为e(n)叠加至s(n-1)的权重；k₁₂为

叠加至s(n-1)的权重；s(n)为惯性高度两次修正后信号，s(1)为s(n)在第一个采样时刻的值，设置为0；

步骤S60，对得到的两次修正后的惯性高度信号，以及高度表经非线性低通滤波后的信号，进行组合卡尔曼滤波，得到组合后的高度信号最优值如下：

pp₁(n)＝pp(n-1)+Q_ww；

bb(n)＝pp₁(n)·[pp₁(n)+R_ww]^-1；

h_k(n)＝s(n)+bb(n)·[h_a(n)-s(n)]；

其中pp(n)为卡尔曼滤波器中的误差协方差信号，pp(1)为pp(n)在第一个采样点的值；bb(n)为卡尔曼增益，pp₁(n)为误差协方差的预测值；Q_ww为设置的预测模型本身的噪声，R_ww为设置的测量噪声；h_k(n)为组合后的高度信号最优值。

Images