ardupilot开发 --- EKF篇

干了这碗汤

已于 2024-01-19 11:48:43 修改

阅读量415

点赞数

分类专栏：无人机/建模/PX4/Adurpilot/Pixhawk 文章标签： Ardupilot

于 2023-08-28 15:56:31 首次发布

本文链接：https://blog.csdn.net/weixin_43321489/article/details/132540381

版权

无人机/建模/PX4/Adurpilot/Pixhawk 专栏收录该内容

106 篇文章 38 订阅

订阅专栏

0. 状态估计的算法发展

在这里插入图片描述

1. 参数调试

AHRS_EKF_USE: set to “1” to use the EKF, “0” to use DCM for attitude control and inertial nav (Copter-3.2.1) or ahrs dead reckoning (Plane) for position control. In Copter-3.3 (and higher) this parameter is forced to “1” and cannot be changed.

AHRS_EKF_TYPE: set to “2” to use EKF2 for attitude and position estimation, “3” for EKF3.

EK2_ENABLE, EK3_ENABLE: set to “1” to enable the EKF2 and/or EKF3 respectively.

EK2_IMU_MASK, EK3_IMU_MASK: a bitmask specifying which IMUs (i.e. accelerometer/gyro) to use. An EKF “core” (i.e. a single EKF instance) will be started for each IMU specified.

1: starts a single EKF core using the first IMU
2: starts a single EKF core using only the second IMU
3: starts two separate EKF cores using the first and second IMUs respectively

EK3_PRIMARY: selects which “core” or “lane” is used as the primary. A value of 0 selects the first IMU lane in the EK3_IMU_MASK, 1 the second, etc. Be sure that the selected primary lane exists. See Affinity and Lane Switching below.

2. 其他常用参数

EK2_ALT_SOURCE which sensor to use as the primary altitude source

0 : use barometer (default)

1 : use range finder. Do not use this option unless the vehicle is being flown indoors where the ground is flat. For terrain following please see copter and plane specific terrain following instructions which do not require changing this parameter.

2 : use GPS. Useful when GPS quality is very good and barometer drift could be a problem. For example if the vehicle will perform long distance missions with altitude changes of >100m.

EK2_ALT_M_NSE: Default is “1.0”. Lower number reduces reliance on accelerometers, increases reliance on barometer.

EK2_GPS_TYPE: Controls how GPS is used.

0 : use 3D velocity & 2D position from GPS

1 : use 2D velocity & 2D position (GPS velocity does not contribute to altitude estimate)

2: use 2D position

3 : no GPS (will use optical flow only if available)

EK2_YAW_M_NSE: Controls the weighting between GPS and Compass when calculating the heading. Default is “0.5”, lower values will cause the compass to be trusted more (i.e. higher weighting to the compass)

3. 亲和度、EKF lanes 切换

有的飞控有冗余的IMU，每个IMU运行一个EKF3实例（线程，成为lane），但只有一个主lane作为AHRS的来源，某个IMU发生故障时可切换主lane.

4. 代码开发相关

Home 或者 EKF origin 是不是位置控制器使用的坐标系的原点？？
Home：The vehicles “Home” position is the location (specified as a latitude, longitude and altitude above sea level) that the vehicle will return to in RTL mode.
一般home点会自动设置为点击解锁是飞机的位置，但可以随时在飞行过程中更改！
EKF origin：EKF 原点，一般设置后不会再更改，此位置通常设置为GPS提供高质量位置后不久的车辆位置（This location is normally set to the vehicle’s location soon after the GPS provides a good quality location）；
home点或ekf原点只要已被外部改变，飞控就会向所有mavlink系统发送下面的massage，来通报这一变更：

HOME_POSITION：home 点被更改；
GPS_GLOBAL_ORIGIN：ekf 原点被更改；
这使得能够在本地坐标系和全球（GPS）坐标系之间进行转换；

5. EKF origin

设置ekf原点：这使得能够在local系和全球（GPS）坐标系之间进行转换；
方法：发送 SET_GPS_GLOBAL_ORIGIN 消息；
GPS_GLOBAL_ORIGIN 即ekf原点，即local坐标系原点；
注意与home点的区别，home可以随时更改，ekf origin 一旦设定就不会更改（只限于本次上电）；

6. EKF1 ? EKF2？ EKF3?

6.1 一些重要的概念

Innovation：其实可以认为是预测值和测量值的误差；原文：When a GPS measurement arrives, the filter calculates the difference between the predicted position from 4) and the position from the GPS. This difference is called an ‘Innovation’.

6.2 EKF1

EKF2,3是EKF1的扩展，EKF1是基础；
EKF算法根据以下传感器：

陀螺仪（测角度信息）
加速度计（测加速度、速度、位置，加速度积分后得到速度和位置但不准）
指南针（磁力计，测偏航信息）
GPS （测经纬度，海拔高度，空速，甚至可以测偏航信息）
空速计（测速度）
气压计（测海拔高度）

来估计（基于EKF坐标系，代码中称为local坐标系，即原点固定在地球一般取上电后GPS第一次得到清晰位置信息时的点）：

位置信息
速度信息
角度信息

共估计 22 states.

源码：AP_NavEKF2.cpp and AP_NavEKF3.cpp

EKF的理论介绍：https://github.com/priseborough/InertialNav/blob/master/derivations/GenerateEquations22states.m

EKF的状态估计步骤简述（非数学描述）：

（0）先定义几个重要的坐标系：
- 地球坐标系、GWS84、GPS坐标系、global frame；经度，纬度，海拔高度；
- 地面坐标系、地固坐标系、局部坐标系、local frame、EKF坐标系；position x y z；
- 机体系、body frame、position x y z；
（1）IMU的陀螺仪测量得到机体系的角速率，积分得到欧拉角（欧拉角是机体系相对于地面坐标系的旋转关系）；
（2）IMU的加速度计测量得到机体系的xyz轴加速度，根据（1）中已知的欧拉角，可以将机体系下的xyz轴加速度转换为地面系的xyz轴加速度；
（3）地面系下的加速度积分得到速度信息；
（4）地面系下的速度积分得到位置信息；
（1）~（4）被称为状态预测State Prediction，有些状态可以预测（可以通过物理定律得到预测方程DotX=AX+Bu），如：
位置，速度，加速度，角度，角速度；
有些状态不可以预测（没有预测方程）：
陀螺仪偏差、加速度计偏差、风速、罗盘偏差，地球磁场等；
这些不可预测的状态通可以通过传感器测量值结合协方差矩阵的一系列计算得到；
（5）The ‘Innovation’ from 6), ‘State Covariance Matrix’ from 5), and the GPS measurement error specified by EKF_POSNE_NOISE are combined to calculate a correction to each of the filter states. This is referred to as a ‘State Correction’.
https://ardupilot.org/dev/docs/extended-kalman-filter.html