EKF: Miscellaneous yaw estimator improvements #14191
Conversation
priseborough
force-pushed
the
pr-yawEstFix
branch
from
3f344d9
to
adb44b7
Compare
|
There is work required to harmonise the AP_Compass and AP_Declination libraries to prevent a situation where they are returning inconsistent information if COMPASS_AUTODEC = 0, but I will tackle that in a separate PR. This was highlighted when trying to achieve flyaway testing by manually setting COMPASS_DEC. Edit: Fix submitted here #14196 |
|
A couple of quick flight tests using EKF3 and EK3_MAG_CAL = 7.
This PR does not address the issue with getting preflight checks to pass when the magnetometer is disabled and EKF2 is being used. |
|
we discussed briefly on the call but will review over the next week or so. |
libraries/AP_NavEKF2/AP_NavEKF2.cpp
Outdated
| @@ -245,7 +245,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { | |||
|
|
|||
| // @Param: MAG_CAL | |||
| // @DisplayName: Magnetometer default fusion mode | |||
| // @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states, when it will use a simpler magnetic heading fusion model that does not use magnetic field states and when it will use an alternative method of yaw determination to the magnetometer. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK2_MAG_MASK parameter. NOTE: limited operation without a magnetometer or any other yaw sensor is possible by setting the COMPASS_USE parameters to 0. If this is done, the EK2_GSF_RUN and EK2_GSF_USE masks must be set to the same as EK3_IMU_MASK. | |||
| // @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states, when it will use a simpler magnetic heading fusion model that does not use magnetic field states and when it will use an alternative method of yaw determination to the magnetometer. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK2_MAG_MASK parameter. NOTE: limited operation without a magnetometer or any other yaw sensor is possible by setting all COMPASS_USE, COMPASS_USE2, COMPASS_USE3, etc parameters to 0 with COMPASS_ENABLE set to 1. If this is done, the EK2_GSF_RUN and EK2_GSF_USE masks must be set to the same as EK3_IMU_MASK. | |||
There was a problem hiding this comment.
I think that these parameter names are different.
EK3_MAG_CAL -> EK2_MAG_CAL
EK3_IMU_MASK -> EK2_IMU_MASK
EK2_GSF_RUN -> EK2_GSF_RUN_MASK
EK2_GSF_USE -> EK2_GSF_USE_MASK
|
@rmackay9 ping |
priseborough
force-pushed
the
pr-yawEstFix
branch
from
4d30eea
to
e64d786
Compare
|
I've added the fixes for #14654 The following results are from SITL testing of a hand launch without use of the magnetomer. EK3_ENABLE = 1 SITL was started using "sim_vehicle.py --frame plane-throw --console --map -L CMAC_PILOTSBOX" and launched using RC 7 2000 to trigger the throw after the "APM: EKF3 IMU0 origin set" message was received. Here are the console messages and GPS innovations before the fix showing the delayed yaw alignment and larger than desirable velocity innovations: 2020-06-22 17:17:27 ArduPlane V4.1.0dev (07ad479)
After the fixes, the reset is a second after launch and the innovations are small due to the more accurate yaw: 2020-06-22 18:37:04 ArduPlane V4.1.0dev (e64d786b)
|
libraries/AP_NavEKF2/AP_NavEKF2.cpp
Outdated
| @@ -245,7 +245,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { | |||
|
|
|||
| // @Param: MAG_CAL | |||
| // @DisplayName: Magnetometer default fusion mode | |||
| // @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states, when it will use a simpler magnetic heading fusion model that does not use magnetic field states and when it will use an alternative method of yaw determination to the magnetometer. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK2_MAG_MASK parameter. NOTE: limited operation without a magnetometer or any other yaw sensor is possible by setting the COMPASS_USE parameters to 0. If this is done, the EK2_GSF_RUN and EK2_GSF_USE masks must be set to the same as EK3_IMU_MASK. | |||
| // @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states, when it will use a simpler magnetic heading fusion model that does not use magnetic field states and when it will use an alternative method of yaw determination to the magnetometer. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK2_MAG_MASK parameter. NOTE: limited operation without a magnetometer or any other yaw sensor is possible by setting all COMPASS_USE, COMPASS_USE2, COMPASS_USE3, etc parameters to 0 with COMPASS_ENABLE set to 1. If this is done, the EK2_GSF_RUN and EK2_GSF_USE masks must be set to the same as EK3_IMU_MASK. | |||
|
@priseborough @tridge i‘m following this closely as i do fly my trad. FW airframes without mag pretty exclusively. i‘ve done a lot of flights on EK3 now and did notice that after launching it takes a couple of seconds for EK3 yaw to align (2-4 secs usually), as compared to just a few ms on EK2. however, in real-flight conditions (hand launch) i can‘t seem to find the same large innovations and delay that SITL produces: EK3 hardware: EK2 hardware: EK3 SITL: is this just coincidental? on a sidenote i wonder if there‘s a more intuitive way to handle yaw-source on no-mag setups using EK3 than having to set _MAG_CAL = 7. there‘s been no separate setting required on EK2 too besides unchecking MAGn_USE. the current EK3 handling imho might be prone to cause relevant support effort. |
This PR should fix the delayed yaw alignment with EKF3. |
|
@vierfuffzig I can modify the EKF3 params so that the EKF-GSF yaw use is enabled automatically if compass is disabled. I think you are right that having to set it to a specific value will increase support queries. |
|
@priseborough thanks, i think moving GSF to auto will add a lot of convenience. |
|
I've added patch to make the use of the GPS for yaw automatic if COMPASS_USEx and COMPASS_ENABLE are set to 0. This has been tested in SITL using ../Tools/autotest/sim_vehicle.py --console --map --frame plane-throw -L CMAC_PILOTSBOX to perform a hand launch to the west. EK3_MAG_CAL was set to 0 as was COMPASS_USEx and COMPASS_ENABLE. The Copter use case was also tested in SITL with COMPASS_USEx and COMPASS_ENABLE set to 0. A takeoff in ALT_HOLD was performed, followed by horizontal movement to align the yaw angle. @vierfuffzig If you are able to do testing with your setup and share the results, then that would be appreciated. |
|
@priseborough thanks a lot, that's great! i might get the chance to get some flights in later today, will report back. |
|
@priseborough are you 100% positive that COMPASS_ENABLE needs to be set to 1 on EK2 for no-mag use? |
|
@priseborough tested this on EK3 with
fast yaw alignment within a few ms into flight, MAG_CAL on default works. excellent, thanks a lot! |
|
I did some testing with my Nano Goblin today running a MatekF765 wing board and on one launch ran into a condition where the yaw aligned rapidly, but the use of GPS was delayed a few seconds due to the bias state convergence check. Commit 4799fba fixes this. |
|
@priseborough did a couple of launches on my nanoTalon yesterday, will take a closer look at each log, don‘t think i ran into delayed GPS fusion though from what i saw on OSD. plus i think i missed a previous PR of yours that already fixed the requirement to set MAG_CAL = 7 explicitely. sorry bout that. |
Instruction now explicitly specifies that all COMPASS_USE parameters must be set to 0 and COMPASS_ENABLE must be set to 1
The setting of EKF state variances is only required when commencing or recommencing velocity fusion. The function that resets the EKF and GSF class variables has been renamed to be more consistent with its function.
Enables yaw bias to be learned when sitting stationary on ground.
Enables yaw bias to be learned when sitting stationary on ground.
The EKF can build up large yaw errors on ground so it is safer to stop using GPS and re-align after launch as per first launch.
Don't require user to separately set EK3_MAG_CAL to fly without a magnetomer
Fixed wing should not wait for bias state convergence after in-flight yaw alignment
priseborough
force-pushed
the
pr-yawEstFix
branch
from
c5d2620
to
1f1a941
Compare
|
I've rebased and fixed the conflict in the AP_NavEKF2.cpp EK2_MAG_CAL parameter description |
|
Well, it passed CI. |
Fixes the one frame delay in processing velocity measurements.
Updates EKF2_MAG_CAL param description to remove ambiguity when flying without compass use.
Eliminates unnecessary reset operations and state variables and uses more descriptive reset function name and description.
Reduces post takeoff EKF-GSF yaw drift due to large yaw gyro bias.
Fixes #14654
Result from SITL test to verify yaw estimator operation with second reset induced by land, disarm, arm and takeoff again.
Yaw estimates:
Weights:
Velocity innovations:
Also