Yaw/Altitude estimation sensor fusion

[shota3527]

With horizon drift fixed. now the biggest threat is unreliable compass/baro added with some accelerometer/gyro vibration
These two sensor needs some extra caution. And sometimes hard to make them work as intended due to various reasons. Sensor failure can easily lead to flyaway
Barometer can also fail due to temperature/moisture/static electricity. A example of a baro faliure:
https://fb.watch/nTQduITxRo/
I personally have three outdoor hand watches, and the baro failure rate is 50%-100% in my mountaineering activities.
GPS is a preferred data source, our model can not make its way home without it

• Heading estimation using both GPS and compass for both airplanes and multi-rotors. Can perform GPS yaw estimation on multi-rotor even it is moving sideways/backwards
• Allows compass-less/baro-less navigation on multi-rotor if mode configured through cli
• Soft rejects compass data when the magnetic field strength is unhealthy. Yaw stays there even when a magnet is near the compass.
• position estimator pos.z, vel.z uses both baro and GPS for more precise estimation, also means less weight on acc, which should improve the skyrocket issue on quad.
• Ignore baro if the difference between baro and GPS altitude is too large (20-30m), baro has a higher failure rate, and model cant fly home if GPS fails
• Raise the default position estimator GPS altitude/vertical velocity gain inav_w_z_gps_p,inav_w_z_gps_v from 10% to about 40% of the horizontal gain. should improve skyrocket issue on quad. BTW GPS is about 3 times more accurate horizontally than vertically
• Fixes Ignore acceleration in estimated position when clipping or high vibration #4681 acc data is set to 0 in position estimation when clipping is detected in the accelerometer. Skyrocket happens because the accelerometer value set to 0 equals velocity/position by the accelerometer is always 0. Now it reduces the weight of the accelerometer correctly by boosting GPS/baro weights.
• Dynamically adjust gps/baro gains by measured vibration in position estimator
• FIX magnetic field strength bug in realflight SITL
• Fixes display issue in configurator sensors tab magnetometer value
• Add measured vibration accvib into black box log
• remove redundant config inav_w_xyz_acc_p
• Sync the update of ground course estimation with position estimation, fixes the issue of fixed-wing position controller using stale/jumped data.
• Added more ublox gps dynamic model and changed the default to air<2G from air<1G, This will affect the filters inside the gps unit, air<1g have tracking problems in acrobatics

Tested on bench with a real FC and in SIM
https://youtu.be/5jrwQeegBQ4
Indoor altitude hold hover test with baro is working

Thinking about whether a filter on the compass is needed or not -> not needed in a reasonable compass placement

How to test

GPS is required, please share your experiences with Blackbox log if possiable

yaw part

Calibrate the compass the way you like and have a look at the measured magnetic filed XYZ in the configurator sensor tab in different orientations of the quad/airplane. A li-ion pack can throw the values completely off.
Enable the compass first, it should have some improvements in yaw estimation since GPS data is introduced and dynamic weight adjustment on the compass is introduced.
And try compass-disabled flying. do some cruising to get an estimation first, followed by some acrobatics to try to confuse GPS yaw estimation

[Jetrell]

@shota3527
I will state that the quad I used to test with is mechanically clean and free of gyro/Acc noise. And is also well filtered.

I firstly ran some tests with the quad having the magnetometer enabled, using this software... It performed Poshold, RTH and a WP mission as expected.
I then proceeded to turn OFF the magnetometer. And progressively went through those tests again. And if it wasn't for the OSD Heading indicator taking time to appear, as the minimum GPS ground speed was obtained.. I would hardly have known it wasn't using the magnetometer!
It was practically perfect. With only the occasional small drift, as a mode was enabled, and GPS straight line speed was increasing (20mtrs tops. It took no time at all to be rid of the slight drift)

• I ran a straight line for 80m, then switch to RTH. It then flew home and landed.
• I then started a 5 point WP mission.. Which it followed 95% as well as with a mag.
• I let it hover in Poshold for about a minute. And also spun it around on the Yaw axis. Then flew tight circles to try and confuse the IMU.. Then after releasing the sticks.. It settle back to a Poshold hover again, with no noticeable yaw drift.
• I also tested MC CRUISE mode.. It too worked fine. And came back to a precise hover, once the quad was stopped, with the yaw holding its heading.

(Edited : Fixedwing navigation is now improved, as seen below)

I can't help wondering if adding the nav_fw_vel_xy controller as @breadoven spoke of, would help with this.

[breadoven]

I can't help wondering if adding the nav_fw_vel_xy controller as @breadoven spoke of, would help with this.

I was only thinking about using velocity for altitude, z, control. FW uses heading for xy control which I guess is just xy velocities resolved to a direction in the end.

How did you switch the compass off for the multirotor tests, in flight switch or disabled when disarmed ? I assume if you disable it when disarmed you need to fly at speed briefly after arming before using Nav modes ?

[Jetrell]

I was only thinking about using velocity for altitude, z, control. FW uses heading for xy control which I guess is just xy velocities resolved to a direction in the end.

I have tried tuning the nav_fw_pos_hdg and nav_fw_heading_p controller at different time in the past.. With mixed results, depending on the airframe.

I assume if you disable it when disarmed you need to fly at speed briefly after arming before using Nav modes ?

Yep.. It was turn off before the flights.. And once armed, it acted just like a fixedwing. Requiring a min GPS speed before the yaw heading registered.

[Jetrell]

@shota3527 You were right.. The Position and Heading controllers were receiving stale data most of the time.
3a9624b has now made a big improvement to WP and RTH trackback navigation for fixedwings... It now tracks much straighter between waypoint points in the wind.. The wind was 20-30km/h with its direction coming from the bottom left of the images.
And Loiter circuits are the tightest I've ever seen them.

Before changes

With changes

The Airplane has a magnetometer installed to take advantage of these software changes.
These are the settings I used.

nav_fw_control_smoothness = 6
nav_fw_bank_angle = 50
nav_fw_climb_angle = 28
nav_wp_radius = 400
nav_fw_wp_tracking_accuracy = 4
nav_fw_wp_tracking_max_angle = 35
nav_fw_wp_turn_smoothing = ON-CUT

With these ones being tuned to suit the changes. (for this plane anyway)

nav_fw_pos_z_p = 48
nav_fw_pos_z_i = 6
nav_fw_pos_z_d = 16
nav_fw_pos_xy_p = 65
nav_fw_pos_xy_i = 6
nav_fw_pos_xy_d = 7
nav_fw_heading_p = 108
nav_fw_pos_hdg_p = 46
nav_fw_pos_hdg_i = 5
nav_fw_pos_hdg_d = 7

[JulioCesarMatias]

As I said on discord, AHRS now goes from a "projection in the horizontal plane to get a yaw" to a 3D fusion (Which is the most correct for aerial vehicles).

I'm happy with these improvements occurring, thanks @shota3527 for the PR, and also thanks to @Jetrell for the tests performed...

In my opinion this is ready to be merged!

[shota3527]

As I said on discord, AHRS now goes from a "projection in the horizontal plane to get a yaw" to a 3D fusion (Which is the most correct for aerial vehicles).

I'm happy with these improvements occurring, thanks @shota3527 for the PR, and also thanks to @Jetrell for the tests performed...

In my opinion this is ready to be merged!

This PR still separates baro and AHRS. i think it should be better to not use position estimator data in AHRS calculation
For the compass part, it was and it is 3D calculation
But for the CoG part, it is still a 2D projection. making it 3D should help with fixed-wing EF pitch and roll correction,, it is already managed by an accelerometer and i need to get direction of the coordinates of ahrs, gps, wind correct when implementing it.

[shota3527]

Added a fade out to ignore baro to prevent similar issue in
#9311

Still need to test if a similar issue happens when posEstimator.gps.epv is near the positionEstimationConfig()->max_eph_epv