[发明专利]一种对高度数据进行处理的方法

权利要求书

1.一种对高度数据进行处理的方法，其特征在于，包括滤波处理的步骤，

a、已知初始时刻的状态值X(k-1)，和此状态对应的协方差值P(k-1)；

b、根据状态转移矩阵A，得到下一时刻k的状态预测值X(k|k-1)；

c、根据测量矩阵H、下一时刻k的状态预测值X(k|k-1)和下一时刻k的测量值Z(k)，计算得出当前时刻的测量余量Y(k)；

d、根据状态转移模型的噪声协方差Q(k)，计算下一时刻k的预测值对应的协方差P(k|k-1)；

e、根据测量模型中的噪声方差R(k)，以及上一步得到的预测值对应的协方差P(k|k-1)，计算得出测量余量协方差S(k)，进而得出增益矩阵K(k)；

f、根据上一步的增益矩阵K(k),得出下一时刻k的估计值X(k)和对应的协方差P(k)；

g、根据上一步得出的数据，重复步骤a-f进行迭代计算。

2.根据权利要求1所述一种对高度数据进行处理的方法，其特征在于，状态转移矩阵A为：T为帧与帧之间的时间间隔；下一时刻的状态预测值为：

X(k|k-1)＝AX(k-1|k-1)。

3.根据权利要求1所述一种对高度数据进行处理的方法，其特征在于，测量矩阵H为H＝[1 0]，当前k时刻的测量余量为，

Y(k)＝Z(k)-H(k)X(k|k-1)。

4.根据权利要求1所述一种对高度数据进行处理的方法，其特征在于，噪声协方差Q(k)为：

a_max为最大的加速度值；下一时刻k的预测值对应的协方差P(k|k-1)为：

P(k|k-1)＝AP(k-1|k-1)A^T+Q(k)。

5.根据权利要求4所述一种对高度数据进行处理的方法，其特征在于，余量协方差S(k)：S(k)＝H(k)P(k|k-1)H^T(k)+R(k)，其中R为测量对应的噪声协方差；

进而得出增益矩阵K(k)为：

K(k)＝P(k|k-1)H^T(k)S^-1(k)。

6.根据权利要求4所述一种对高度数据进行处理的方法，其特征在于，下一时刻k的估计值X(k)为：

X(k|k)＝X(k|k-1)+K(k)Y(k)

对应的协方差P(k)为：

P(k|k)＝(I-K(k)H(k))P(k|k-1)。

7.根据权利要求2所述一种对高度数据进行处理的方法，其特征在于，本方法还包括数据平滑处理的步骤，如下：