CN109765402A - 一种基于双加速度计的加速度测量装置和卡尔曼滤波算法 - Google Patents

Abstract

本发明涉及一种基于双加速度计的加速度测量装置和卡尔曼滤波算法，测量装置包括探测球和计算机，探测球内置经过球体中心的圆形PCB板，PCB板上设有两个加速度测量芯片、一个角速度测量芯片，角速度测量芯片设置于PCB板圆心位置，两个加速度测量芯片对称设置于角速度测量芯片的两侧，探测球用于固定在运动物体上、采集运动物体的加速度数据和角速度数据，计算机用于对采集到的加速度数据初步滤除噪声信号以及卡尔曼滤波，得到滤除噪声信号后的加速度数据。本发明通过对双加速度计芯片进行自相关算法运算，使用卡尔曼滤波方法最大限度的降低误差的存在，得出更加可靠的加速度数值及其实时的变化情况，满足系统及用户需求。

Description

一种基于双加速度计的加速度测量装置和卡尔曼滤波算法

技术领域

本发明涉及一种加速度数据处理算法，特别是一种基于双加速度计的加速度测量装置和卡尔曼滤波算法。

背景技术

加速度数据常用来衡量物体的运动过程及状态的指数，它与物体的形状、密度、受力状态、驱动情况等因素有关。在四旋翼飞行器、汽车定位、运动状态模拟等惯导系统过程中，通过实时测量物体在运动过程中每一时刻的加速度数据变化，就可以实时掌握、模拟出物体的运动状态。对一些物体，无法直接观测其运动状态，可以通过本发明来处理加速度信号，得到真实可靠的加速度数据，整合模拟出物体的运动状态与运动轨迹，具有非常重要的应用价值。

目前得到加速度数据主要是直接法。这种方法主要通过加速度计直接输出数据，将加速度数据进行数据融合以及积分计算，来得出物体的运动状态。但这种方法容易受到条件和操作影响，误差较大；加速度计在输出数据时，由于自身存在噪声信号，会造成加速度数值的波动，信噪比过大的加速度计输出数值在积分之后，噪声会被指数式放大，严重影响对物体运动状态的判断以及运动轨迹的模拟。加速度计尽管已经校准了零漂，但是自身的噪声信号仍然无法抑制；经历了算法的积分运算之后，噪声信号就会呈现出指数式放大，淹没了有效信号，得出了实验数据就无法被用来模拟物体的运动状态。在民用级别的系统中，由于元器件的精度和设备本身系统的稳定性的不足，导致测量出来的数据精度较低，民用级别的元器件的噪声特性往往会伴随着环境湿度、温度、物体的运动状态的变化而发生显著改变。传统的姿态解算方法在上述条件无法获得最优的结果，甚至在姿态结算过程中，发生误差淹没有效信号的情况。

发明内容

针对以上不足，本发明提供了一种基于双加速度计的加速度测量装置和卡尔曼滤波算法，通过对双加速度计芯片进行自相关算法运算，以及使用卡尔曼滤波方法最大限度的降低误差的存在，得出更加可靠的加速度数值。

本发明的技术方案为：

一种基于双加速度计的加速度测量装置，包括探测球和计算机，所述探测球内置经过球体中心的圆形PCB板，所述PCB板上设有两个加速度测量芯片、一个角速度测量芯片、供电装置、WIFI串口和控制开关，所述角速度测量芯片设置于PCB板圆心位置，所述两个加速度测量芯片对称设置于角速度测量芯片的两侧，所述WIFI串口无线连接计算机；所述探测球用于固定在运动物体上、采集运动物体的加速度数据和角速度数据，所述计算机用于对采集到的加速度数据初步滤除噪声信号以及卡尔曼滤波，得到滤除噪声信号后的加速度数据。

还包括水平台，所述水平台用于在静置状态下通过计算机校正探测球的零漂现象。

所述角速度测量芯片为陀螺仪。

所述探测球为塑料制，直径为6cm。

一种利用上述加速度测量装置的卡尔曼滤波算法，包括以下步骤：

S1：探测球采集到运动物体的加速度数据和角速度数据，通过WIFI串口将数据无线传送到计算机，所述加速度数据包括两个加速度测量芯片采集到的两组加速度数据；

S2：计算机对采集到的加速度数据和角速度数据进行姿态解算，将载体坐标系下的加速度、角速度转换成地球坐标系的加速度、角速度；

S3：对两组加速度数据中的前100个采样点分别求均值，用1号加速度计的所有数值减去1号加速度计前100个采样点的均值，2号加速度计的所有数值减去2号加速度计前100个采样点的均值，得到初步滤除噪声信号后的加速度数据；

S4：构建系统的状态方程和观测方程，所述状态方程表达式为：

X_1(k)＝A×X_1(k-1)+B×U_1(k-1)

X_2(k)＝A×X_2(k-1)+B×U_2(k-1)

上式代表从k-1时刻到k时刻两个加速度计的状态变化过程，其中，X_1(k)、X_2(k)分别代表1号加速度计、2号加速度计在第k时刻采集到的加速度数据，X_1(k-1)、X_2(k-1)分别代表1号加速度计、2号加速度计在第k-1时刻采集到的加速度数据，U_1(k-1)、U_2(k-1)分别代表1号加速度计、2号加速度计在第k-1时刻的噪声信号数据，A、B分别代表系统的状态转移矩阵和噪声驱动矩阵；

所述观测方程的表达式为：

Z_1(k)＝H×X_1(k-1)+V_1(k)

Z_2(k)＝H×X_2(k-1)+V_2(k)

上式代表在第k时刻从系统状态转换到传感器观测数值的转换过程，其中，Z_1(k)、Z_2(k)分别代表1号加速度计、2号加速度计在第k时刻的传感器观测值，X_1(k-1)、X_2(k-1)分别代表1号加速度计、2号加速度计在第k-1时刻采集到的加速度数据，V_1(k)、V_2(k)分别代表1号加速度计、2号加速度计在第k时刻的观测噪声信号数据，H代表系统的观测矩阵；

S5：对两组加速度数值进行卡尔曼滤波，具体包括以下步骤：

S51：对两个加速度计分别进行预测运算，通过第k-1时刻的加速度最佳预测值预测第k时刻的加速度，加速度的预测过程表达式为：

X_{1_pre(k)}＝A×X_kf(k-1)+B×U_1(k-1)

P_{1_pre(k)}＝A×P_kf(k-1)×A'+Q₁

X_{2_pre(k)}＝A×X_kf(k-1)+B×U_2(k-1)

P_{2_pre(k)}＝A×P_kf(k-1)×A'+Q₂

其中，X_{1_pre(k)}、X_{2_pre(k)}分别代表1号加速度计、2号加速度计在第k时刻的加速度预测值，U_1(k-1)、U_2(k-1)分别代表1号加速度计、2号加速度计在第k-1时刻的噪声信号数据，P_{1_pre(k)}、P_{2_pre(k)}分别代表1号加速度计、2号加速度计的协方差，Q₁、Q₂分别代表1号加速度计、2号加速度计噪声信号的方差值，X_kf(k-1)代表第k-1时刻的加速度最佳预测值，P_kf(k-1)代表第k-1时刻的协方差最佳预测值，A、B分别代表状态转移矩阵和噪声驱动矩阵，A'为A的矩阵转置；

S52：比较两个加速度计的协方差大小，将较小的协方差赋值给P_pre(k)，并将对应加速度计的第k时刻加速度预测值赋值给X_pre(k)，将对应加速度计的第k时刻传感器观测值赋值给Z_(k)；

S53：卡尔曼增益Kg(k)的表达式为：

K_g(k)＝P_pre(k)×H'×inv(H×P_pre(k)×H'+R)

式中，H代表观测矩阵，H'为H的矩阵转置，R代表系统的测量误差；

S54：将卡尔曼增益K_g(k)、第k时刻加速度的预测值X_pre(k)、第k时刻传感器的观测值Z_(k)联立，得到第k时刻的加速度最佳预测值X_kf(k)，表达式为：

X_kf(k)＝X_pre(k)+K_g(k)×(Z_(k)-H×X_pre(k))；

S55：系统更新第k时刻的协方差最佳预测值P_kf(k)，表达式为：

P_kf(k)＝(I-K_g(k)×H)×P_pre(k)

式中，I代表单位矩阵维度；

S56：重复步骤S51-S55，依次计算第k+1时刻、第k+2时刻、…、第k+n时刻加速度的最佳预测值。

所述步骤S1前还包括以下步骤：将探测球固定于水平台上，打开控制开关，通过计算机校正探测球的零漂现象。

所述步骤S4中还包括以下步骤：根据四元数方程得到系统的状态转移矩阵和噪声驱动矩阵，根据快速高斯-牛顿法得到系统的观测矩阵。

本发明的技术方案具有以下有益效果：

本发明的加速度测量装置采用双加速度计芯片的组合，双加速计芯片分别测量出系统的加速度数据，采集到的有效信号是相同的，噪声信号是具有差别的，通过对双加速度计芯片进行自相关算法运算，即对两组加速度数值的前100个采样点进行均值处理，再用所有加速度数值分别减去该均值，就可以初步滤除噪声信号，提高加速度数据的精确性。

在算法过程中，通过卡尔曼滤波算法，通过上一时刻的加速度最佳预测值来预测当前时刻的加速度值，并且使用当前时刻的加速度预测值与当前时刻的加速度观测值进行数据融合，得出当前时刻的加速度最佳预测值，当前时刻的加速度最佳预测值用来预测下一时刻的加速度值，重复上述步骤，依次计算出下一时刻的加速度最佳预测值。卡尔曼滤波算法使得噪声信号进一步减小，最大限度的降低误差的存在，得出精确有效的加速度数值及其实时的变化情况，满足系统及用户需求。

本发明采用双加速度计的测量装置，以及算法中使用卡尔曼滤波算法，所处理得到的加速度信号具有精确可靠、快速高效、方便等特点。

附图说明

图1为本发明的PCB板结构视图；

图2为本发明的卡尔曼滤波算法流程图。

具体实施方式

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。

本发明的基于双加速度计的加速度测量装置，包括探测球、水平台和计算机，探测球为塑料制，直径为6cm，探测球为两个半球拧合而成，其中一个半球的径面为圆形PCB板10，另外一个半球只有球面无径面。参照图1，PCB板10上设有两个加速度测量芯片11、一个角速度测量芯片12、供电装置(未显示)、WIFI串口(未显示)和控制开关(未显示)，角速度测量芯片12为陀螺仪(型号为L3GD20TR)，设置于PCB板10圆心，两个加速度测量芯片11(型号为LIS3DSHTR)对称设置于角速度测量芯片12的两侧，WIFI串口无线连接计算机，供电装置用于给芯片供电，控制开关用于接通或断开供电装置。

PCB板平面可以视作为xoy平面，角速度测量芯片12位于PCB板10圆心、即探测球的几何重心位置，用于测量三轴角速度数据，探测球在x轴、y轴、z轴方向上的旋转角度。双加速度测量芯片11对称设置于圆心两侧，用于测量三轴的加速度数据。双加速度计是以探测球为载体，所构建的载体坐标系以PCB板所在平面作为xoy面，双加速度测量芯片11的连线可视作x轴，通过角速度测量芯片12的垂线可视作y轴，z轴垂直于PCB板。

双加速度计测量基本原理为：测量装置由两个相同的加速度计构成，输出的信号在理想状态下应是完全相符合的，如果在测量过程中，发现二者的输出数据不同，应该是由于加速度计芯片自身的噪声信号所导致的。因此比较二者的输出加速度数据，可以提取出来有效信号，减小系统的噪声干扰。

使用前，将探测球的两个半球拧开，打开控制开关，加速度测量芯片11和角速度测量芯片12开始运行。将两个半球拧合，将探测球固定在运动物体上，芯片开始采集运动物体的加速度数据和角速度数据，所述数据通过无线方式发送到计算机，计算机程序对采集到的加速度数据初步滤除噪声信号以及卡尔曼滤波，得到滤除噪声信号后的加速度数据。水平台用于在静置状态下通过计算机校正探测球的零漂现象。

本发明的加速度测量装置的卡尔曼滤波算法，包括以下步骤：

步骤一：将探测球固定于水平台上，打开控制开关，通过计算机校正探测球的零漂现象，缩小测量误差；

步骤二：将探测球固定在运动物体上，探测球随运动物体同时运动，采集到运动物体的加速度数据和角速度数据，通过WIFI串口将数据无线传送到计算机，其中，加速度数据包括两个加速度测量芯片采集到的两组加速度数据；

步骤三：计算机程序对采集到的加速度数据和角速度数据进行姿态解算，将载体坐标系下的三轴加速度、角速度转换成地球坐标系的三轴加速度、角速度。加速度计采集到的三轴加速度为a＝[ax,ay,az]^T，其中ax、ay、az分别为载体坐标系中x轴、y轴、z轴的加速度数据，陀螺仪采集到的三轴角速度为w＝[wx,wy,wz]^T；其中wx、wy、wz为载体坐标系中x轴、y轴、z轴的角速度数据。将加速度数据、角速度数据通过姿态解算，转换成关于地球坐标系的三轴加速度、角速度。

步骤四：对两组加速度数据中的前100个采样点分别求均值，用1号加速度计的所有数值减去1号加速度计的均值，2号加速度计的所有数值减去2号加速度计的均值，得到初步滤除噪声信号后的加速度数据。实验开始的前100个采样点的这段时间内，系统处于从静止状态过渡到运动状态的启动过程，此启动过程受到噪声信号的影响。通过求取均值，可以将此平均值视作为噪声信号，并将后续所有的采样数据减去这个均值，可以有效的减小噪声信号的影响。

计算过程表达式为：

a_mean_1＝mean(ax_1(1,1:100))

ax_1(i)＝ax_1(i)-a_mean_1

a_mean_2＝mean(ax_2(1,1:100))

ax_2(i)＝ax_2(i)-a_mean_2

其中，ax_1(i)、ax_2(i)分别代表1号加速度计、2号加速度计的测量数据，a_mean_1、a_mean_2分别代表1号加速度计、2号加速度计的前100个采样点的加速度平均值。

步骤五：构建系统的状态方程和观测方程，具体包括：

5.1、根据四元数方程得到系统的状态转移矩阵和噪声驱动矩阵，根据快速高斯-牛顿法得到系统的观测矩阵；

5.2、构建系统的状态方程，表达式为：

X_1(k)＝A×X_1(k-1)+B×U_1(k-1)

X_2(k)＝A×X_2(k-1)+B×U_2(k-1)

上式代表从k-1时刻到k时刻两个加速度计的状态变化过程，其中，X_1(k)、X_2(k)分别代表1号加速度计、2号加速度计在第k时刻采集到的加速度数据，X_1(k-1)、X_2(k-1)分别代表1号加速度计、2号加速度计在第k-1时刻采集到的加速度数据，U_1(k-1)、U_2(k-1)分别代表1号加速度计、2号加速度计在第k-1时刻的噪声信号数据，A、B为系统参数，A为状态转移矩阵，代表从k-1时刻到k时刻系统状态的变化，B为噪声驱动矩阵，代表从k-1时刻到k时刻噪声的变化，A、B的数值根据系统标定得来。

5.3、构建系统的观测方程，表达式为：

Z_1(k)＝H×X_1(k-1)+V_1(k)

Z_2(k)＝H×X_2(k-1)+V_2(k)

上式代表在第k时刻从系统状态转换到传感器观测数值的转换过程，其中，Z_1(k)、Z_2(k)分别代表1号加速度计、2号加速度计在第k时刻的传感器观测值，即可以被人为测量出来的观测数值，X_1(k-1)、X_2(k-1)分别代表1号加速度计、2号加速度计在第k-1时刻采集到的加速度数据，V_1(k)、V_2(k)分别代表1号加速度计、2号加速度计在第k时刻的观测噪声信号数据，H为系统的观测矩阵，代表从加速度计采集的加速度数据转换到传感器观测值的中间变量。

步骤六：对两组加速度数值进行卡尔曼滤波，参考图2，具体包括以下步骤：

6.1：对两个加速度计分别进行预测运算，通过第k-1时刻的加速度最佳预测值预测第k时刻的加速度，加速度的预测过程表达式为：

X_{1_pre(k)}＝A×X_kf(k-1)+B×U_1(k-1)

P_{1_pre(k)}＝A×P_kf(k-1)×A'+Q₁

X_{2_pre(k)}＝A×X_kf(k-1)+B×U_2(k-1)

P_{2_pre(k)}＝A×P_kf(k-1)×A'+Q₂

其中，X_{1_pre(k)}、X_{2_pre(k)}分别代表1号加速度计、2号加速度计在第k时刻的加速度预测值，U_1(k-1)、U_2(k-1)分别代表1号加速度计、2号加速度计在第k-1时刻的噪声信号数据，P_{1_pre(k)}、P_{2_pre(k)}分别代表1号加速度计、2号加速度计的协方差，Q₁、Q₂分别代表1号加速度计、2号加速度计噪声信号的方差值，X_kf(k-1)代表第k-1时刻的加速度最佳预测值，P_kf(k-1)代表第k-1时刻的协方差最佳预测值，A、B为系统参数，分别代表状态转移矩阵和噪声驱动矩阵，A'为A的矩阵转置；

6.2：比较两个加速度计的协方差大小，将较小的协方差赋值给P_pre(k)，并将对应加速度计的第k时刻加速度预测值赋值给X_pre(k)，将对应加速度计的第k时刻传感器观测值赋值给Z_(k)，协方差P_pre(k)赋值表达式为：

P_pre(k)＝min(P_{1_pre(k)},P_{2_pre(k)})；

6.3：卡尔曼增益K_g(k)的表达式为：

K_g(k)＝P_pre(k)×H'×inv(H×P_pre(k)×H'+R)

式中，H代表系统的观测矩阵，H'为H的矩阵转置，R代表系统的测量误差；

6.4：将卡尔曼增益K_g(k)、第k时刻加速度的预测值X_pre(k)、第k时刻传感器的观测值Z_(k)联立，得到第k时刻的加速度最佳预测值X_kf(k)，表达式为：

X_kf(k)＝X_pre(k)+K_g(k)×(Z_(k)-H×X_pre(k))；

6.5：系统更新第k时刻的协方差最佳预测值P_kf(k)，表达式为：

P_kf(k)＝(I-K_g(k)×H)×P_pre(k)

式中，I为单位矩阵维度，维度大小与系统方程有关；

6.6：重复步骤6.1-6.5，依次计算第k+1时刻、第k+2时刻、…、第k+n时刻加速度的最佳预测值。

通过卡尔曼滤波算法，通过上一时刻的加速度最佳预测值来预测当前时刻的加速度值，并且使用当前时刻的加速度预测值与当前时刻的加速度观测值进行数据融合，得出当前时刻的加速度最佳预测值，当前时刻的加速度最佳预测值用来预测下一时刻的加速度值，重复上述步骤，依次计算出下一时刻的加速度最佳预测值。卡尔曼滤波算法使得噪声信号进一步减小，最大限度的降低误差的存在，得出精确有效的加速度数值及其实时的变化情况，满足系统及用户需求。

卡尔曼滤波方法主要应用场景是针对具有数学模型的信号，进行时域上的信号处理，尤其是带有误差扰动的信号。卡尔曼滤波的应用已经广泛应用在导航制导、目标定位、跟踪、天气预报、地质勘探、股票市场预测等等领域，并有着越来越多的扩展发展，向着其他行业领域去延伸。

以上公开的仅为本发明的实施例，但是，本发明并非局限于此，任何本领域的技术人员能思之的变化都应落入本发明的保护范围。

Claims (7)

1.一种基于双加速度计的加速度测量装置，其特征在于，包括探测球和计算机，所述探测球内置经过球体中心的圆形PCB板(10)，所述PCB板(10)上设有两个加速度测量芯片(11)、一个角速度测量芯片(12)、供电装置、WIFI串口和控制开关，所述角速度测量芯片(12)设置于PCB板(10)圆心位置，所述两个加速度测量芯片(11)对称设置于角速度测量芯片(12)的两侧，所述WIFI串口无线连接计算机；所述探测球用于固定在运动物体上、采集运动物体的加速度数据和角速度数据，所述计算机用于对采集到的加速度数据初步滤除噪声信号以及卡尔曼滤波，得到滤除噪声信号后的加速度数据。

2.根据权利要求1所述的基于双加速度计的加速度测量装置，其特征在于，还包括水平台，所述水平台用于在静置状态下通过计算机校正探测球的零漂现象。

3.根据权利要求1所述的基于双加速度计的加速度测量装置，其特征在于，所述角速度测量芯片(12)为陀螺仪。

4.根据权利要求1所述的基于双加速度计的加速度测量装置，其特征在于，所述探测球为塑料制，直径为6cm。

5.一种利用权利要求1-4中任一项所述的加速度测量装置的卡尔曼滤波算法，其特征在于，包括以下步骤：

S1：探测球采集到运动物体的加速度数据和角速度数据，通过WIFI串口将数据无线传送到计算机，所述加速度数据包括两个加速度测量芯片采集到的两组加速度数据；

S2：计算机对采集到的加速度数据和角速度数据进行姿态解算，将载体坐标系下的加速度、角速度转换成地球坐标系的加速度、角速度；

S3：对两组加速度数据中的前100个采样点分别求均值，用1号加速度计的所有数值减去1号加速度计前100个采样点的均值，2号加速度计的所有数值减去2号加速度计前100个采样点的均值，得到初步滤除噪声信号后的加速度数据；

S4：构建系统的状态方程和观测方程，所述状态方程表达式为：

X_1(k)＝A×X_1(k-1)+B×U_1(k-1)

X_2(k)＝A×X_2(k-1)+B×U_2(k-1)

上式代表从k-1时刻到k时刻两个加速度计的状态变化过程，其中，X_1(k)、X_2(k)分别代表1号加速度计、2号加速度计在第k时刻采集到的加速度数据，X_1(k-1)、X_2(k-1)分别代表1号加速度计、2号加速度计在第k-1时刻采集到的加速度数据，U_1(k-1)、U_2(k-1)分别代表1号加速度计、2号加速度计在第k-1时刻的噪声信号数据，A、B分别代表系统的状态转移矩阵和噪声驱动矩阵；

所述观测方程的表达式为：

Z_1(k)＝H×X_1(k-1)+V_1(k)

Z_2(k)＝H×X_2(k-1)+V_2(k)

上式代表在第k时刻从系统状态转换到传感器观测数值的转换过程，其中，Z_1(k)、Z_2(k)分别代表1号加速度计、2号加速度计在第k时刻的传感器观测值，X_1(k-1)、X_2(k-1)分别代表1号加速度计、2号加速度计在第k-1时刻采集到的加速度数据，V_1(k)、V_2(k)分别代表1号加速度计、2号加速度计在第k时刻的观测噪声信号数据，H代表系统的观测矩阵；

S5：对两组加速度数值进行卡尔曼滤波，具体包括以下步骤：

S51：对两个加速度计分别进行预测运算，通过第k-1时刻的加速度最佳预测值预测第k时刻的加速度，加速度的预测过程表达式为：

X_{1_pre(k)}＝A×X_kf(k-1)+B×U_1(k-1)

P_{1_pre(k)}＝A×P_kf(k-1)×A'+Q₁

X_{2_pre(k)}＝A×X_kf(k-1)+B×U_2(k-1)

P_{2_pre(k)}＝A×P_kf(k-1)×A'+Q₂

其中，X_{1_pre(k)}、X_{2_pre(k)}分别代表1号加速度计、2号加速度计在第k时刻的加速度预测值，U_1(k-1)、U_2(k-1)分别代表1号加速度计、2号加速度计在第k-1时刻的噪声信号数据，P_{1_pre(k)}、P_{2_pre(k)}分别代表1号加速度计、2号加速度计的协方差，Q₁、Q₂分别代表1号加速度计、2号加速度计噪声信号的方差值，X_kf(k-1)代表第k-1时刻的加速度最佳预测值，P_kf(k-1)代表第k-1时刻的协方差最佳预测值，A、B分别代表状态转移矩阵和噪声驱动矩阵，A'为A的矩阵转置；

S52：比较两个加速度计的协方差大小，将较小的协方差赋值给P_pre(k)，并将对应加速度计的第k时刻加速度预测值赋值给X_pre(k)，将对应加速度计的第k时刻传感器观测值赋值给Z_(k)；

S53：卡尔曼增益K_g(k)的表达式为：

K_g(k)＝P_pre(k)×H'×inv(H×P_pre(k)×H'+R)

式中，H代表观测矩阵，H'为H的矩阵转置，R代表系统的测量误差；

S54：将卡尔曼增益K_g(k)、第k时刻加速度的预测值X_pre(k)、第k时刻传感器的观测值Z_(k)联立，得到第k时刻的加速度最佳预测值X_kf(k)，表达式为：

X_kf(k)＝X_pre(k)+K_g(k)×(Z_(k)-H×X_pre(k))；

S55：系统更新第k时刻的协方差最佳预测值P_kf(k)，表达式为：

P_kf(k)＝(I-K_g(k)×H)×P_pre(k)

式中，I代表单位矩阵维度；

S56：重复步骤S51-S55，依次计算第k+1时刻、第k+2时刻、…、第k+n时刻加速度的最佳预测值。

6.根据权利要求5所述的卡尔曼滤波算法，其特征在于，所述步骤S1前还包括以下步骤：将探测球固定于水平台上，打开控制开关，通过计算机校正探测球的零漂现象。

7.根据权利要求5所述的卡尔曼滤波算法，其特征在于，所述步骤S4中还包括以下步骤：根据四元数方程得到系统的状态转移矩阵和噪声驱动矩阵，根据快速高斯-牛顿法得到系统的观测矩阵。