Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update EKF2 overview #3533

Merged
merged 4 commits into from
Jan 23, 2025
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion en/SUMMARY.md
Original file line number Diff line number Diff line change
Expand Up @@ -388,7 +388,7 @@
- [PX4 Ethernet Setup](advanced_config/ethernet_setup.md)
- [Standard Configuration](config/index.md)
- [Advanced Configuration](advanced_config/index.md)
- [ECL/EKF Overview & Tuning](advanced_config/tuning_the_ecl_ekf.md)
- [Using PX4's Navigation Filter (EKF2)](advanced_config/tuning_the_ecl_ekf.md)
- [Finding/Updating Parameters](advanced_config/parameters.md)
- [Full Parameter Reference](advanced_config/parameter_reference.md)

Expand Down
2 changes: 1 addition & 1 deletion en/_sidebar.md
Original file line number Diff line number Diff line change
Expand Up @@ -384,7 +384,7 @@
- [PX4 Ethernet Setup](/advanced_config/ethernet_setup.md)
- [Standard Configuration](/config/index.md)
- [Advanced Configuration](/advanced_config/index.md)
- [ECL/EKF Overview & Tuning](/advanced_config/tuning_the_ecl_ekf.md)
- [Using PX4's Navigation Filter (EKF2)](/advanced_config/tuning_the_ecl_ekf.md)
- [Finding/Updating Parameters](/advanced_config/parameters.md)
- [Full Parameter Reference](/advanced_config/parameter_reference.md)

Expand Down
6 changes: 3 additions & 3 deletions en/advanced/computer_vision.md
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ For information about MoCap see:

- [External Position Estimation](../ros/external_position_estimation.md)
- [Flying with Motion Capture (VICON, NOKOV, Optitrack)](../tutorials/motion-capture.md)
- [EKF > External Vision System](../advanced_config/tuning_the_ecl_ekf.md#external-vision-system)
- [Using PX4's Navigation Filter (EKF2) > External Vision System](../advanced_config/tuning_the_ecl_ekf.md#external-vision-system)

## Visual Inertial Odometry (VIO)

Expand All @@ -43,7 +43,7 @@ One difference between VIO and [MoCap](#motion-capture) is that VIO cameras/IMU

For information about configuring VIO on PX4 see:

- [EKF > External Vision System](../advanced_config/tuning_the_ecl_ekf.md#external-vision-system)
- [Using PX4's Navigation Filter (EKF2) > External Vision System](../advanced_config/tuning_the_ecl_ekf.md#external-vision-system)
- [T265 Setup guide](../peripherals/camera_t265_vio.md)

## Optical Flow
Expand All @@ -53,7 +53,7 @@ For information about configuring VIO on PX4 see:
For information about optical flow see:

- [Optical Flow](../sensor/optical_flow.md)
- [EKF > Optical Flow](../advanced_config/tuning_the_ecl_ekf.md#optical-flow)
- [Using PX4's Navigation Filter (EKF2) > Optical Flow](../advanced_config/tuning_the_ecl_ekf.md#optical-flow)

## Comparisons

Expand Down
2 changes: 1 addition & 1 deletion en/advanced_config/index.md
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ This topic lists configuration topics that are not particularly vehicle specific

## Feature configuration

- [ECL/EKF Overview & Tuning](../advanced_config/tuning_the_ecl_ekf.md)
- [Using PX4's Navigation Filter (EKF2)](../advanced_config/tuning_the_ecl_ekf.md)
- [Flight Termination Configuration](../advanced_config/flight_termination.md)
- [Land Detector Configuration](../advanced_config/land_detector.md)
- [Prearm/Arm/Disarm Configuration](../advanced_config/prearm_arm_disarm.md)
Expand Down
2 changes: 1 addition & 1 deletion en/advanced_config/static_pressure_buildup.md
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ Aim for a barometer altitude drop of less than 2 metres at maximum horizontal sp
## Dynamic Calibration

After modifying the hardware, you can then use the [EKF2_PCOEF\_\*](../advanced_config/parameter_reference.md#EKF2_PCOEF_XN) parameters to tune for expected barometer variation based on relative air velocity.
For more information see [ECL/EKF Overview & Tuning > Correction for Static Pressure Position Error](../advanced_config/tuning_the_ecl_ekf.md#correction-for-static-pressure-position-error).
For more information see [Using PX4's Navigation Filter (EKF2) > Correction for Static Pressure Position Error](../advanced_config/tuning_the_ecl_ekf.md#correction-for-static-pressure-position-error).

::: info
The approach works well if the relationship between the error due to static pressure and the velocity varies linearly.
Expand Down
45 changes: 31 additions & 14 deletions en/advanced_config/tuning_the_ecl_ekf.md
Original file line number Diff line number Diff line change
@@ -1,23 +1,27 @@
# Using the ECL EKF
# Using PX4's Navigation Filter (EKF2)

This tutorial answers common questions about use of the ECL EKF algorithm.
This tutorial answers common questions about use of the EKF algorithm used for navigation.

hamishwillee marked this conversation as resolved.
Show resolved Hide resolved
:::tip
The [PX4 State Estimation Overview](https://youtu.be/HkYRJJoyBwQ) video from the _PX4 Developer Summit 2019_ (Dr. Paul Riseborough) provides an overview of the estimator, and additionally describes both the major changes from 2018/2019, and the expected improvements through 2020.
The [PX4 State Estimation Overview](https://youtu.be/HkYRJJoyBwQ) video from the _PX4 Developer Summit 2019_ (Dr. Paul Riseborough) provides an overview of the estimator, and additionally describes both the major changes from 2018/2019, major changes and improvements were added since then.
:::

## What is the ECL EKF?
## Overview

The Estimation and Control Library (ECL) uses an Extended Kalman Filter (EKF) algorithm to process sensor measurements and provide an estimate of the following states:
PX4's Navigation filter uses an Extended Kalman Filter (EKF) algorithm to process sensor measurements and provide an estimate of the following states:

- Quaternion defining the rotation from North, East, Down local earth frame to X, Y, Z body frame
- Quaternion defining the rotation from North, East, Down local navigation frame to X, Y, Z body frame
- Velocity at the IMU - North, East, Down (m/s)
- Position at the IMU - North, East, Down (m)
- IMU delta angle bias estimates - X, Y, Z (rad)
- IMU delta velocity bias estimates - X, Y, Z (m/s)
- Position at the IMU - Latitude (rad), Longitude (rad), Altitude (m)
- IMU gyro bias estimates - X, Y, Z (rad/s)
- IMU accelerometer bias estimates - X, Y, Z (m/s<sup>2</sup>)
- Earth Magnetic field components - North, East, Down \(gauss\)
- Vehicle body frame magnetic field bias - X, Y, Z \(gauss\)
- Wind velocity - North, East \(m/s\)
- Terrain altitude (m)

To improve stability, an "error-state" formulation is implemented
This is especially relevant when estimating the uncertainty of a rotation which is a 3D vector (tangent space of SO(3)).

The EKF runs on a delayed 'fusion time horizon' to allow for different time delays on each measurement relative to the IMU.
Data for each sensor is FIFO buffered and retrieved from the buffer by the EKF to be used at the correct time.
Expand All @@ -27,16 +31,28 @@ A complementary filter is used to propagate the states forward from the 'fusion
The time constant for this filter is controlled by the [EKF2_TAU_VEL](../advanced_config/parameter_reference.md#EKF2_TAU_VEL) and [EKF2_TAU_POS](../advanced_config/parameter_reference.md#EKF2_TAU_POS) parameters.

::: info
The 'fusion time horizon' delay and length of the buffers is determined by the largest of the `EKF2_*_DELAY` parameters.
If a sensor is not being used, it is recommended to set its time delay to zero.
The 'fusion time horizon' delay and length of the buffers is determined by [EKF2_DELAY_MAX](../advanced_config/parameter_reference.md#EKF2_DELAY_MAX).
This value should be at least as large as the longest delay `EKF2\_\*\_DELAY`.
Reducing the 'fusion time horizon' delay reduces errors in the complementary filter used to propagate states forward to current time.
:::

The EKF uses the IMU data for state prediction only.
IMU data is not used as an observation in the EKF derivation.
The algebraic equations for the covariance prediction and measurement jacobians are derived using [SymForce](https://symforce.org/) and can be found here: [Symbolic Derivation](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/ekf2/EKF/python/ekf_derivation/derivation.py).
Covariance update is done using the [Joseph Stabilized form](https://en.wikipedia.org/wiki/Kalman_filter#Deriving_the_posteriori_estimate_covariance_matrix) to improve numerical stability and allow conditional update of independent states.

### Precisions about the position output

The position is estimated as latitude, longitude and altitude and the INS integration is performed using the WGS84 ellipsoid mode.
However, the position uncertainty is defined in the local navigation frame at the current position (i.e.: NED error in meters).

The position and velocity states are adjusted to account for the offset between the IMU and the body frame before they are output to the control loops.

The position of the IMU relative to the body frame is set by the `EKF2_IMU_POS_X,Y,Z` parameters.

The EKF uses the IMU data for state prediction only. IMU data is not used as an observation in the EKF derivation.
The algebraic equations for the covariance prediction, state update and covariance update were derived using the Matlab symbolic toolbox and can be found here: [Matlab Symbolic Derivation](https://github.com/PX4/PX4-ECL/blob/master/EKF/matlab/scripts/Terrain%20Estimator/GenerateEquationsTerrainEstimator.m).
In addition to the global position estimate in latitude/longitude/altitude, the filter also provides a local position estimate (NED in meters) by projecting the global position estimate using an [azimuthal_equidistant_projection](https://en.wikipedia.org/wiki/Azimuthal_equidistant_projection) centred on an arbitrary origin.
This origin is automatically set when global position measurements are fused but can also be specified manually.
If no global position information is provided, only the local position is available and the INS integration is performed on a spherical Earth.

## Running a Single EKF Instance

Expand Down Expand Up @@ -118,7 +134,8 @@ The EKF has different modes of operation that allow for different combinations o
On start-up the filter checks for a minimum viable combination of sensors and after initial tilt, yaw and height alignment is completed, enters a mode that provides rotation, vertical velocity, vertical position, IMU delta angle bias and IMU delta velocity bias estimates.

This mode requires IMU data, a source of yaw (magnetometer or external vision) and a source of height data.
This minimum data set is required for all EKF modes of operation. Other sensor data can then be used to estimate additional states.
This minimum data set is required for all EKF modes of operation.
Other sensor data can then be used to estimate additional states.

### IMU

Expand Down
2 changes: 1 addition & 1 deletion en/computer_vision/motion_capture.md
Original file line number Diff line number Diff line change
Expand Up @@ -16,4 +16,4 @@ For information about MoCap see:

- [Using Vision or Motion Capture Systems for Position Estimation](../ros/external_position_estimation.md). <!-- bring across info into user guide? -->
- [Flying with Motion Capture (VICON, NOKOV, Optitrack)](../tutorials/motion-capture.md). <!-- bring across info into user guide? -->
- [EKF > External Vision System](../advanced_config/tuning_the_ecl_ekf.md#external-vision-system)
- [Using PX4's Navigation Filter (EKF2) > External Vision System](../advanced_config/tuning_the_ecl_ekf.md#external-vision-system)
4 changes: 2 additions & 2 deletions en/computer_vision/visual_inertial_odometry.md
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ The following parameters must be set to use external position information with E

These can be set in _QGroundControl_ > **Vehicle Setup > Parameters > EKF2** (remember to reboot the flight controller in order for parameter changes to take effect).

For more detailed/additional information, see: [ECL/EKF Overview & Tuning > External Vision System](../advanced_config/tuning_the_ecl_ekf.md#external-vision-system).
For more detailed/additional information, see: [Using PX4's Navigation Filter (EKF2) > External Vision System](../advanced_config/tuning_the_ecl_ekf.md#external-vision-system).

<a id="tuning-EKF2_EV_DELAY"></a>

Expand Down Expand Up @@ -167,4 +167,4 @@ This topic also explains how to configure VIO for use with the LPE Estimator (de

## Further Information

- [ECL/EKF Overview & Tuning > External Vision System](../advanced_config/tuning_the_ecl_ekf.md#external-vision-system)
- [Using PX4's Navigation Filter (EKF2) > External Vision System](../advanced_config/tuning_the_ecl_ekf.md#external-vision-system)
4 changes: 2 additions & 2 deletions en/debug/system_wide_replay.md
Original file line number Diff line number Diff line change
Expand Up @@ -118,8 +118,8 @@ When parameters are overridden, corresponding parameter changes in the log are n
This is a specialization of the system-wide replay for fast EKF2 replay.

::: info
The recording and replay of flight logs with [multiple EKF instances](../advanced_config/tuning_the_ecl_ekf.md#running-multiple-ekf-instances) is not supported.
To enable recording for EKF replay you must set the parameters to enable a [single EKF instance](../advanced_config/tuning_the_ecl_ekf.md#running-a-single-ekf-instance).
The recording and replay of flight logs with [multiple EKF2 instances](../advanced_config/tuning_the_ecl_ekf.md#running-multiple-ekf-instances) is not supported.
To enable recording for EKF replay you must set the parameters to enable a [single EKF2 instance](../advanced_config/tuning_the_ecl_ekf.md#running-a-single-ekf-instance).
:::

In EKF2 mode, the replay will automatically create the ORB publisher rules described above.
Expand Down
2 changes: 1 addition & 1 deletion en/flying/pre_flight_checks.md
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ Note that the [Arming Check Report](#qgc-arming-check-report) is a much easier w

## EKF Preflight Checks/Errors

This sections lists errors, with associated checks and parameters, that are reported by the [EKF](../advanced_config/tuning_the_ecl_ekf.md) (and propagate to _QGroundControl_).
This sections lists errors, with associated checks and parameters, that are reported by [EKF2](../advanced_config/tuning_the_ecl_ekf.md) (and propagate to _QGroundControl_).
These are provided for information only (the QGC Arming Checks UI is the best way to get error and solution information).

#### PREFLIGHT FAIL: EKF HIGH IMU ACCEL BIAS
Expand Down
2 changes: 1 addition & 1 deletion en/flying/terrain_following_holding.md
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,7 @@ As the distance to ground changes, the altitude setpoint adjusts to keep the hei
At higher altitudes (when the estimator reports that the distance sensor data is invalid) the vehicle switches to *altitude following*, and will typically fly at a near-constant height above mean sea level (AMSL) using an absolute height sensor for altitude data.

::: info
More precisely, the vehicle will use the available selected sources of altitude data as defined [here](../advanced_config/tuning_the_ecl_ekf.md#height).
More precisely, the vehicle will use the available selected sources of altitude data as defined in [Using PX4's Navigation Filter (EKF2) > Height](../advanced_config/tuning_the_ecl_ekf.md#height).
:::

Terrain following is enabled by setting [MPC_ALT_MODE](../advanced_config/parameter_reference.md#MPC_ALT_MODE) to `1`.
Expand Down
2 changes: 1 addition & 1 deletion en/gps_compass/rtk_gps.md
Original file line number Diff line number Diff line change
Expand Up @@ -212,7 +212,7 @@ For example, you can decrease [EKF2_GPS_V_NOISE](../advanced_config/parameter_re
#### Dual Receivers

A second GPS receiver can be used as a backup (either RTK or non RTK).
See the [EKF2 GPS Configuration](../advanced_config/tuning_the_ecl_ekf.md#gps) section.
See the [Using PX4's Navigation Filter (EKF2) > GPS](../advanced_config/tuning_the_ecl_ekf.md#gps) section.

<!--
- Video demonstration would be nice.
Expand Down
2 changes: 1 addition & 1 deletion en/ros/external_position_estimation.md
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ You can apply this trick with every system - if you need to obtain a NED frame,
## EKF2 Tuning/Configuration

Note: this is a quick overview.
For more detailed information, check the [EKF2 tuning guide](../advanced_config/tuning_the_ecl_ekf.md)
For more detailed information, check the [Using PX4's Navigation Filter (EKF2)](../advanced_config/tuning_the_ecl_ekf.md)

The following parameters must be set to use external position information with EKF2 (these can be set in *QGroundControl* > **Vehicle Setup > Parameters > EKF2**).

Expand Down
2 changes: 1 addition & 1 deletion en/sensor/airspeed.md
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,6 @@ Airspeed sensors should be calibrated by following the instructions: [Basic Conf

## See Also

- [Using the ECL EKF > Airspeed](../advanced_config/tuning_the_ecl_ekf.md#airspeed)
- [Using PX4's Navigation Filter (EKF2) > Airspeed](../advanced_config/tuning_the_ecl_ekf.md#airspeed)
- [Airspeed drivers](https://github.com/PX4/PX4-Autopilot/tree/main/src/drivers/differential_pressure) (source code)
- [VTOL Without an Airspeed Sensor](../config_vtol/vtol_without_airspeed_sensor.md)
2 changes: 1 addition & 1 deletion en/sensor/optical_flow.md
Original file line number Diff line number Diff line change
Expand Up @@ -98,4 +98,4 @@ If your optical flow sensor is offset from the vehicle centre, you can set this
| <a id="EKF2_OF_POS_Y"></a>[EKF2_OF_POS_Y](../advanced_config/parameter_reference.md#EKF2_OF_POS_Y) | Y position of optical flow focal point in body frame (default is 0.0m). |
| <a id="EKF2_OF_POS_Z"></a>[EKF2_OF_POS_Z](../advanced_config/parameter_reference.md#EKF2_OF_POS_Z) | Z position of optical flow focal point in body frame (default is 0.0m). |

See [Using the ECL EKF > Optical flow](../advanced_config/tuning_the_ecl_ekf.md#optical-flow) for more information.
See [Using PX4's Navigation Filter (EKF2) > Optical flow](../advanced_config/tuning_the_ecl_ekf.md#optical-flow) for more information.
Loading