matlab陀螺仪飞行器滤波补偿,PX4开发指南-4.9.ecl EKF

在飞行过程中，EKF 的输出(outputs), 状态(states) 以及状态数据(status data)会发布到许多uORB话题(Topic)中，这些话题会存储到SD卡中。 下面的介绍假设数据已经以.ulog文件的形式被记录，为了使用.ulog格式，需要将 SYS_LOGGER 参数置1。

.ulog格式文件可以在python中使用PX4 pyulog library来进行分析。

大部分 EKF 数据都在.ulog文件中的 ekf2_innovations和 estimator_status 这两个uORB消息里面。

Output Data(输出数据)

控制回路反馈数据在 control_state 消息中。

States(状态)

查看estimator_status中的states[32]。状态的索引如下：

[0... 3] 四元数

[4 ... 6] 速度 NED (m/s)

[7 ... 9] 位置 NED (m)

[10 ... 12] IMU 角度增量偏差 XYZ (rad)

[13 ... 15] IMU 速度增量偏差 XYZ (m/s)

[16 ... 18] 地球磁场 NED (gauss)

[19 ... 21] 机体磁场 XYZ (gauss)

[22 ... 23] 风速 NE (m/s)

[24 ... 32] 未使用

State Variances(状态方差)

参考 estimator_status 中的covariances[28](协方差)。 covariances[28]的索引图如下:

[0 ... 3] 四元数

[4 ... 6] 速度 NED (m/s)^2

[7 ... 9] 位置 NED (m^2)

[10 ... 12] IMU 角度增量偏差 XYZ (rad^2)

[13 ... 15] IMU 速度增量偏差 XYZ (m/s)^2

[16 ... 18] 地球磁场 NED (gauss^2)

[19 ... 21] 机体磁场 XYZ (gauss^2)

[22 ... 23] 风速 NE (m/s)^2

[24 ... 28] 未使用

观测新息(Observation Innovations)

磁力计 XYZ (gauss) : 参考 mag_innov3 ，位于 ekf2_innovations。

偏航角 (rad) : 参考 heading_innov ，位于 ekf2_innovations.

速度和位置新息 : 参考 vel_pos_innov6 ，位于 ekf2_innovations。vel_pos_innov6 的索引图如下:

[0 ... 2] 速度 NED (m/s)

[3 ... 5] 位置 NED (m)

真实风速 (m/s) : 参考 airspeed_innov ，位于 ekf2_innovations。

合成侧滑 (rad) : 参考 beta_innov in ekf2_innovations。

光流 XY (rad/sec) : 参考 flow_innov in ekf2_innovations。

地面上方高度 (m) : 参考 hagl_innov in ekf2_innovations。

观测信息方差

磁力计 XYZ (gauss^2) : 参考 mag_innov_var3 ，位于 ekf2_innovations。

偏航角 (rad^2) : 参考 heading_innov_var，位于the ekf2_innovations message.

速度和位置新息 : 参考 vel_pos_innov_var6，位于ekf2_innovations。 vel_pos_innov6 的索引图如下:

[0 ... 2] 速度 NED (m/s)^2

[3 ... 5] 位置 NED (m^2)

真实风速 (m/s)^2 : 参考 airspeed_innov_var ，位于 ekf2_innovations。

合成侧滑 (rad^2) : 参考 beta_innov_var ，位于 ekf2_innovations。

光流 XY (rad/sec)^2 : 参考 flow_innov_var ，位于 ekf2_innovations。

地面上方高度 (m^2) : 参考 hagl_innov_var ，位于 ekf2_innovations。

Output Complementary Filter(输出互补滤波器)

输出互补滤波用于将状态从融合时间范围推进到当前时间。要查看融合时间范围内测量的角度、速度和位置追踪误差量级(magnitude)，参考 ekf2_innovations 中的 output_tracking_error3索引图如下：

[

1 速度追踪误差量级 (m/s)。 可以通过调整参数 EKF2_TAU_VEL来调整速度追踪时间常数。降低这个参数会降低稳态误差，但是会增加NED速度输出的观测噪声数量。

2 位置追踪误差量级 (m)。 可以通过调整参数 EKF2_TAU_POS来调整位置追踪时间常数。降低这个参数会降低稳态误差，但是会增加NED位置输出的观测噪声数量。

EKF 误差

EKF对于恶劣状态下的状态和协方差更新内置了误差检测。 参考 filter_fault_flags ，位于 estimator_status。

观测误差

有两种类型的观测错误:

数据丢失。一个例子就是测距仪不能提供返回值。

新息，就是状态估计和传感器观测之间的差异超出了。一个例子就是过多的震动导致大的垂直位置误差，导致气压高度测量值被拒绝。

这些都会导致观测数据被拒绝足够长时间而导致EKF尝试使用传感器观测值重置状态。所有的观测都有一个统计置信度检测应用于新息。检测的标准差的数目受到每种观测类型的参数 EKF2_<>_GATE 的控制。

测试水平(Test levels)在 estimator_status 中，如下所示:

mag_test_ratio : 最大磁力计新息分量与新息测试限制之比

vel_test_ratio : 最大速度新息分量与新息测试限制之比

pos_test_ratio : 最大水平位置新息分量与新息测试限制之比

hgt_test_ratio : 最大垂直位置新息分量与新息测试限制之比

tas_test_ratio : 最大真实风速新息分量与新息测试限制之比

hagl_test_ratio : 最大地面上方高度新息分量与新息测试限制之比

对于每个传感器的二进制 pass/fail 汇总, 参考 innovation_check_flags ，位于 estimator_status。

GPS 质量检测

开始GPS辅助之前，EKF进行了一系列的GPS质量检测。这些检测由参数 EKF2_GPS_CHECK 和 EKF2_REQ<>控制。这些检测的 pass/fail 状态记录在 estimator_status.gps_check_fail_flags 消息中。当所有要求的GPS检测通过，这个整数将为0。如果EKF没有开始GPS校准，查看 estimator_status 中的 gps_check_fail_flags 位掩码的定义。

EKF 数值误差

为了降低对处理器的要求，对所有的运算EKF使用单精度浮点类型，对协方差预测和更新方程使用一阶近似。这意味着当重新调试EKF时有可能遇到异常情况，在其中协方差矩阵操作条件变得很恶劣以至于导致状态估计中产生发散或者明显的误差。

为了阻止这个，每一个协方差和状态更新步骤包含以下误差检测和修正步骤：

如果新息方差小于观测方差(这要求一个负的状态方差，这是不可能的)或者协方差更新将对任何状态产生负的方差：

状态和协方差更新被跳过

协方差矩阵中相应的行和列被重置

失败被记录在 estimator_status filter_fault_flags 消息中

状态方差 (协方差矩阵中的对角线) 被限制成非负值

上限被应用到状态方差中去

协方差矩阵强制对称

重新调试滤波器后，像降低噪声变量、estimator_status.gps_check_fail_flags 的数值这样的部分重新调试应该再次检测以确保仍然为零。