[发明专利]一种采用动态积分卡尔曼滤波的混合高度测量方法

权利要求书

1.一种采用动态积分卡尔曼滤波的混合高度测量方法，其特征在于，包括以下步骤：

步骤S10，在载体上安装SiA200 MEMS加速度传感器，并进行积分得到惯性速度与惯性高度；在载体上安装GPS接收器，测量载体GPS高度；在载体上安装无线电高度表，测量载体的无线电高度如下：

v＝∫adt；

h＝∫vdt；

其中a(n)对应的是时间t＝n*ΔT时刻采用SiA200 MEMS加速度传感器测量的载体加速度信号，简写为a，其中ΔT为数据的输出采样周期，dt表示对时间信号的积分，v为惯性速度信号，h为惯性高度信号；在载体上安装FYC-LG24-800型无线电高度表，测量载体高度，记作yw，称为无线电高度信号，然后按照加速度计的数据间隔周期ΔT，将高度数据进行离散化，得到数据y_w(n)，其中n＝1,2,3…，对应的是时间t＝n*ΔT时刻的无线电高度信号数据；在载体上安装GPS接收器，测量载体GPS高度，记作y_g，称作GPS高度信号，然后按照加速度计的数据间隔周期ΔT，将高度数据进行离散化，得到数据y_g(n)，其中n＝1,2,3…，对应的是时间t＝n*ΔT时刻的GPS高度信号数据；

步骤S20，设计二阶开方非线性平滑滤波器，将所述的惯性高度信号数据通过二阶开方非线性平滑滤波器，得到惯性高度平滑信号；然后将无线电高度信号数据通过二阶开方非线性平滑滤波器，得到无线电高度平滑信号；最后将GPS高度信号数据，通过二阶开方非线性平滑滤波器，得到GPS平滑信号如下：

e_h＝h(n)-h_l(n)；

e_h1＝y_w(n)-y_wl(n)；

e_h12＝y_g(n)-y_gl(n)；

其中T₁、T₂、T₃为滤波器常值参数；k₁、k₂、k₃、k₄、k₅、k₆为滤波器增益参数，为常数；ε₁为滤波器开方柔化系数，为常值；h₁为惯性高度一阶微分信号；h_l1为惯性平滑高度一阶微分信号；h_l2为惯性平滑高度二阶微分信号；e_h为惯性高度滤波误差信号；h_l为惯性高度平滑信号；y_w1为无线电高度一阶微分信号；y_wl1为无线电平滑高度一阶微分信号；y_wl2为无线电平滑高度二阶微分信号；e_h1为无线电高度滤波误差信号；y_wl为无线电高度平滑信号；y_g1为GPS高度一阶微分信号；y_gl1为GPS平滑高度一阶微分信号；y_gl2为GPS平滑高度二阶微分信号；e_h2为GPS高度滤波误差信号；y_gl为GPS高度平滑信号；

步骤S30，将GPS平滑信号与无线电高度平滑信号进行组合卡尔曼滤波，得到GPS无线电组合卡尔曼滤波高度信号，分别对组合卡尔曼滤波高度信号求解高度误差，再进行非线性积分，得到误差积分信号，再进行协方差信号在线解算，调节卡尔曼滤波器如下：

p_w1(n)＝p_w(n-1)+Q_w1；

b_w(n)＝p_w1(n)·[p_w1(n)+R_w1]^-1；

y_wgk(n)＝y_wl(n)+b_w(n)·[y_gl(n)-y_wl(n)]；

e_a1(n)＝y_wgk(n)-y_wl(n)；

e_a2(n)＝y_wgk(n)-y_gl(n)；

其中p_w(n)为卡尔曼滤波器中的无线电误差协方差信号，p_w(1)为p_w(n)在第一个采样点的值；b_w(n)为卡尔曼无线电增益，p_w1(n)为无线电误差协方差的预测值；Q_w1为无线电前验方差，其初始值选取为常值，R_w1为无线电观测方差，其初始值选取为常值；y_wgk(n)为GPS无线电组合卡尔曼滤波高度信号；e_a1(n)为无线电组合高度误差信号；e_a2(n)为GPS组合高度误差信号；s_a1为无线电组合高度误差积分信号与s_a2为GPS组合高度误差积分信号；ε₂为常值参数信号；ε₃为常值参数信号；k_a1、k_a2、k_a3、k_a4为常值参数信号；

步骤S40，将GPS平滑信号与惯性高度平滑信号进行组合卡尔曼滤波，得到GPS惯性组合卡尔曼滤波高度信号，分别对合卡尔曼滤波高度信号求解高度误差，再进行非线性积分，得到误差积分信号，再进行协方差信号在线解算，调节卡尔曼滤波器如下：

p_i1(n)＝p_i(n-1)+Q_i1；

b_i(n)＝p_i1(n)·[p_i1(n)+R_i1]^-1；

y_igk(n)＝y_il(n)+b_i(n)·[y_gl(n)-y_il(n)]；

e_b1(n)＝y_igk(n)-y_il(n)；

e_b2(n)＝y_igk(n)-h_l(n)；

其中p_i(n)为卡尔曼滤波器中的惯性误差协方差信号，p_i(1)为p_i(n)在第一个采样点的值；b_i(n)为卡尔曼惯性增益，p_i1(n)为惯性误差协方差的预测值；Q_i1为惯性前验方差，其初始值选取为常值，R_i1为惯性观测方差，其初始值选取为常值；y_igk(n)为GPS惯性组合卡尔曼滤波高度信号；e_b1(n)为惯性组合高度误差信号；e_b2(n)为GPS惯性组合高度误差信号；s_b1为惯性组合高度误差积分信号与s_b2为GPS惯性组合高度误差积分信号；k_a5、k_a6、k_a7、k_a8为常值参数信号；

步骤S50，将惯性平滑信号与无线电高度平滑信号进行组合卡尔曼滤波，得到惯性无线电组合卡尔曼滤波高度信号，分别对合卡尔曼滤波高度信号求解高度误差，再进行非线性积分，得到误差积分信号，再进行协方差信号在线解算，调节卡尔曼滤波器如下：

p_wi1(n)＝p_wi(n-1)+Q_wi1；

b_wi(n)＝p_wi1(n)·[p_wi1(n)+R_wi1]^-1；

y_wik(n)＝y_wl(n)+b_wi(n)·[h_l(n)-y_wl(n)]；

e_c1(n)＝y_wik(n)-h_l(n)；

e_c2(n)＝y_wik(n)-y_wl(n)；

其中p_wi(n)为卡尔曼滤波器中的惯性无线电误差协方差信号，p_wi(1)为p_wi(n)在第一个采样点的值；b_wi(n)为卡尔曼惯性无线电增益，p_wi1(n)为惯性无线电误差协方差的预测值；Q_wi1为惯性无线电前验方差，其初始值选取为常值，R_wi1为惯性无线电观测方差，其初始值选取为常值；y_wik(n)为GPS惯性无线电组合卡尔曼滤波高度信号；e_c1(n)为惯性相对组合误差信号；e_c2(n)为无线电相对组合误差信号；s_c1为惯性相对组合误差信号积分信号与s_c2为无线电相对组合误差信号积分信号；ε₃为常值参数信号；k_a9、k_a10、k_a11、k_a12为常值参数信号；

步骤S60，对所述的三种组合卡尔曼滤波高度信号进行动态组合，得到优化组合高度信号，并求解三种组合卡尔曼滤波高度信号对优化组合高度信号的误差信号，进行非线性积分后并叠加相应方差信号，实时生成动态组合加权因子，对三种卡尔曼滤波高度进行优化组合后得到最终高度输出总信号如下：

e_w1(n)＝y_o(n)-y_wgk(n)+Q_w1(n)+R_w1(n)；

e_w2(n)＝y_o(n)-y_igk(n)+Q_i1(n)+R_i1(n)；

e_w3(n)＝y_o(n)-y_wik(n)+Q_wi1(n)+R_wi1(n)；

d₁₁(n+1)＝d₁₁(n)+e_w1(n)ΔT；

d₁₂(n+1)＝d₁₂(n)+e_w2(n)ΔT；

d₁₃(n+1)＝d₁₃(n)+e_w3(n)ΔT；

y_o(n)＝d₁y_wgk(n)+d₂y_igk(n)+d₃y_wik(n)；其中e_w1(n)为无线电GPS总误差信号；e_w2(n)为惯性GPS总误差、e_w3(n)为无线电惯性总误差信号；d₁₁为无线电GPS总误差积分信号、d₁₂为惯性GPS总误差积分信号、d₁₃为无线电惯性总误差积分信号；d₁为无线电GPS加权因子、d₂为惯性GPS加权因子、d₃为无线电惯性加权因子；y_o(n)为高度输出总信号。