AHRS centrifugal force compensation by gps

[shota3527]

Background

When the plane perform other than 1.00G maneuver, There will be drifts because the orientation measured from the accelerometer is incorrect. We adjust the weight between the gyro and accelerometer orientation measurements. the gyro can measure the rotation rate which can be converted to orientation, accelerometer can measure the orientation by measuring the gravity vector.

Two kinds of error will happen, it may happen at same time

1. When the weight of the accelerometer is too high, the horizon drifts to the way the plane turns.
2. When the weight of the accelerometer is too low, the gyro error can not be corrected, the horizon drifts in random directions in long run(the gyro error won't grow too much in short run)

When the plane continuously perform other than 1.00G maneuver, it will be impossible to correct the error unless other sensors are used.

Problem description

This PR aims to end the drift of current ahrs with minimal changes required by enhancing the current one The current dcm ahrs implementation in 5.1.0 is great as long as not facing its downsides, it has the following advantages:

• The code is simple and effective
• Internally uses quaternion to express rotations, it requires less resource to compute than the rotation matrix
• Has two mechanisms accWeight_Nearness accWeight_RateIgnore to dynamically adjust the accelerometer weight in fast movements which gravity estimations from the accelerometer are less reliable.
• it is proven by many users

However there also downsides in the current implementation.

1. It do not compensate the centrifugal/inertia force in accelerometer data on speed/direction changes, This is the main reason causing horizon drift theologically. For example, if the plane is turning right, there will be inertia force to left, and ahrs will consider left as the direction to the ground, same can be said in throttle up/down to back/front. This can be fixed by using the gps to calculate the inertia force
2. fixed-wing yaw estimation by GPS ground course does not take wind into account, it will consider the ground course as yaw, not the direction fixed-wing is pointing. it can be fixed by just enabling the magnetometer or introducing wind compensation. incorrect yaw will result in incorrect centrifugal force estimation.

Features

1. Can prevent the horizon error(drift) from growing and correct the drift in hover, flying straight, and slow to moderate maneuvers. It is done by centrifugal/inertia force compensation by GPS. In fast-aggresive movements, the reflash rate/lag/accuracy of GPS seems to be not enough for compensation
2. When centrifugal/inertia force compensation by GPS is not available, fall back to the behavior of 5.1.0. Can only Prevent and correct drift in hover or flying straight
3. Similar to 5.1.0, The weight shift curve of from compensated accelerometer to gyro can but adjusted to prevent error introduced by the accelerometer in fast-aggressive movements(you can also turn the weight shifting off), However, there will be a scale applied to weight more on compensated accelerometer when gps is available.
4. Accelerometer weight shift mechanism based on turn rate will work correctly in turbulence, changed turn rate LPF to work on XYZ axis separately.
5. Soft rejects accelerometer update when estimated gravity after compensation is not 1G, In case of noisy incoming data/GPS lags/GPS lost/bad heading estimation etc.
6. Wind compensation in yaw estimation by GPS. Can be turned on or off by cli
7. Added LPF for the body orientation used in wind estimation, The smooth input and the lag introduced are helpful for wind estimator

Progress:

• [x] implement
• [x] test in simulator
• [x] test in realflight
• [ ] mechanism/parameter adjusting and more tests
• [ ] Review

Experiments

loiter comparison in simulator with 'attitude copied from x plane' https://youtu.be/8vwQHSIKAZ8

dart 250g flight test in one take off 3-5g acro: little error in acrobatic movements, 7+min 1.3g 45deg banking loiter: no noticeable horizon drift There might be a bug that RateIgnore was completely disenabled in this video https://www.youtube.com/watch?v=FDUFIy0nUsQ

Horizon get corrected while continuously turning on a noisy build https://youtu.be/egDqsVJlafw

Test in multi-rotors https://youtu.be/CeOf-HswdBQ

Test notes

on commit id cab91c14af8d32eda30d40d4e9b471a54635c110 i recommend following parameters as a starting point:

set imu_acc_ignore_rate=15
# Decide the weight shift curve of from compensated accelerometer to gyro
# higher number means accelerometer is used in faster turns
set imu_acc_ignore_slope=5
set imu_gps_yaw_windcomp=ON # wind compensation will improve ahrs estimation if it is working correctly
set imu_dcm_kp= 2000 
# The general weight on the accelerometer, higher weight means ahrs recovers faster when drifted
# but may introduce more drift when turning
set imu_dcm_ki=0 #or 50 if drift occurs after flying for several minutes 
set imu_dcm_ki_mag=2500 # the value 5000 in 5.1.0 might be too high
set gps_provider=UBLOX7 #if your GPS is supported, a higher GPS reflash rate helps 

Turn on the wind estimator in OSD and see how wind estimator performs Good calibration on the accelerometer, soft mount flight controller, reduce electrical noise to get a better result

Problems

• [x] There are wind estimation problems in my 5.1.0 based build in real flights -> solved by applying osd fixing commits
• [ ] GPS vertical velocity might not be accurate enough for compensation -> test shows turn compensation off on Z axis is not a good idea, barometer may be helpful

---

Original post

trying to fix the horizon drift caused by centrifugal force by using the velocity changes from gps. since I am not familiar with inav/c development, any advice is welcome This pr will replace pr #4371

waiting for opportunities to test outside

[0crap]

Awesome effort for having a look at this. 👍

[DzikuVx]

OK, I'm super eager to see what will come out of it!

[b14ckyy]

Sounds interesting. If it is based on current master and the HITL support is in there as well, I can test fly it in the simulator in about 3 hours.

[b14ckyy]

Tested commit 682a762. I saw not a big difference to the existing AHRS with mitigations turned off. And a very slow recovery. https://www.youtube.com/watch?v=swVVFoLQuZo Blackbox log if helpful: LOG00050.zip

[shota3527]

thank you very much b14ckyy, if possible, I think the following condition will make a better comparison. these parameters will make the imu relay more on accelerometer than gyroscope. set imu_acc_ignore_rate = 20
set imu_acc_ignore_slope = 5 set imu_dcm_kp = 5000 set imu_dcm_ki = 50 and put the plane into poshold for about 3-5 minutes and see difference

[b14ckyy]

@shota3527 Thanks, I will check that after work today. I wonder why Ignore rate and slope are still needed. I expectee that we don't need these anymore as these are the mitigations to disable AHI realignment during turns. With proper centrifugal force compensation this should not be needed?

Also please check out this PR here to avoid collissions and unnecessary double work https://github.com/iNavFlight/inav/pull/8407

[shota3527]

I have run it in the sim, and it is working as expected tested 4fca7ec724fa0a350689dbad2af78b04ce6f60df .This commit is HIL enabled. https://youtu.be/cK39xbSQris

The following settings is only for comparison between two videos, please feel free to try other parameters set imu_dcm_kp = 5000 set imu_dcm_ki = 50 set imu_acc_ignore_rate = 20 set imu_acc_ignore_slope = 5

The imu_acc_ignore_rate and imu_acc_ignore_slope mechanism is still functional. in the case of GPS-lost/GPS-latency/fast-movements, we can reduce the weight of the accelerometer. However, the ignore amount is reduced(accelerometer weight increased) when GPS compensation is available.

The compensation is available when inav_use_gps_velned is set to ON(default) it still has some work to be done

• [x] find/verify the time source correspond to GPS velocity,i think native GPS time is a good option,but i don't know weather they are updated at same time. <-The naza gps can not get native GPS time milliseconds, use the local time millis() instead
• [x] ~~commit 4fca7ec724fa0a350689dbad2af78b04ce6f60df have disabled some sanity checks in order to run it in sim~~ sanity checks on gps time related things is not necessary when use the local time
• [x] flight test

[b14ckyy]

OK thanks for the explanation. That makes sense. Will check it out after work again, so about 4 hours to go :D

btw can we have a quck chat soon? FB, Discord, Skype, Telegram or so?

[shota3527]

@b14ckyy I prefer FB It is 0000am in Tokyo now...

[shota3527]

OK, I'm super eager to see what will come out of it!

@DzikuVx we are having an ahrs can withstand the most aggressive movements pulled out of a plane and no drifts in 30-45deg bank angle loiter for 7+ mins https://www.youtube.com/watch?v=FDUFIy0nUsQ Only little modification to the original repository, also minimal computational resources increase i have also updated the main post, please check

[rts18]

@shota3527
I've viewed your test video's. While its impressive not to see accelerometer drift. Are these tests conclusive? Wouldn't it be better to aim for turns closer to 1G, to prove your implementation can mitigate the drift? When using any version of INAV, with a clean build. I never experience accelerometer drift when performing higher than 1.1G maneuvers. The times I experience it more frequently on both platforms. Is when slow Yaw input is applied to the model, when its also banked on the roll axis. Or more specific to fixed wings. Its often induced in slower turns, conversely to more responsive turns.

[shota3527]

@shota3527 I've viewed your test video's. While its impressive not to see accelerometer drift. Are these tests conclusive? Wouldn't it be better to aim for turns closer to 1G, to prove your implementation can mitigate the drift? When using any version of INAV, with a clean build. I never experience accelerometer drift when performing higher than 1.1G maneuvers. The times I experience it more frequently on both platforms. Is when slow Yaw input is applied to the model, when its also banked on the roll axis. Or more specific to fixed wings. Its often induced in slower turns, conversely to more responsive turns.

i am planning to carry out more tests and continue developing the code i just prefer test to test them in more difficult situations, more test/improvements are coming

This pr primarily aims at slow turns, especially for loiter more than 5 minutes, it will fall back to similar behavior of 5.1.0 in high G moves. if you perform other than 1.00G maneuver in 5.1.0, there will be drifts because the orientation measured from the accelerometer is incorrect. 5.1.0 implementation will soft ignore the orientation from the accelerometer. Will result in either kind of error: 1. The ignore is not enough, the horizon drifts to the way the plane turns 2. The ignore is too much, the gyro error can not be corrected, The horizon drifts in random directions in long run(the gyro error wont grow too much in short run)

[JulioCesarMatias]

well...you got there! I always knew that the original iNav AHRS only needed two things: Gyro Drift Correction, and Acc Drift Correction (centrifugal). But I preferred to write a new AHRS (maybe I was wrong on that point). But good job, I'm hoping this works!

[JulioCesarMatias]

"i_limit" too exists in my PR of the new AHRS, how did you develop this equation? In my PR, I believe my equation is incorrect...

But, I'll try with yours to see the result

[shota3527]

"i_limit" too exists in my PR of the new AHRS, how did you develop this equation? In my PR, I believe my equation is incorrect... But, I'll try with yours to see the result

Thanks, my equation is from a view of PI controller. When error angle exceeds X deg, then the P correction can always exceed i correction.

I also did a test using MPU 6000 imu, Used a hair dryer to heat it from 15 celsius to about 50 celsius, I see about 1 deg/s gyro drift. which is close to the equation

[0crap]

@shota3527 Is the build folder 20220930 on your Google drive still the latest and greatest? So we can have some fun giving it a flight test. Thx!

Edit: Maybe it's even more convenient to use this .hex? https://github.com/iNavFlight/inav/actions/runs/3219097718

[shota3527]

@shota3527 Is the build folder 20220930 on your Google drive still the latest and greatest? So we can have some fun giving it a flight test. Thx!

Edit: Maybe it's even more convenient to use this .hex? https://github.com/iNavFlight/inav/actions/runs/3219097718

@0crap I have made some changes today(1010) on documents and default values. it is a kind of same with 0930. https://github.com/iNavFlight/inav/actions/runs/3219097718 This link will be more convenient

[0crap]

Awesome, thx!

[DzikuVx]

Hi. That is awesome news . I will have to check when I will get some time thought

pt., 23 wrz 2022, 14:50 użytkownik shota3527 @.***> napisał:

OK, I'm super eager to see what will come out of it!

@DzikuVx https://github.com/DzikuVx we are having an ahrs can withstand the most aggressive movements pulled out of a plane and no drifts in 45deg bank angle loiter for 7+ mins https://www.youtube.com/watch?v=FDUFIy0nUsQ Only little modification to the original repository, also minimal computational resources increase i have also updated the main post, please check

— Reply to this email directly, view it on GitHub https://github.com/iNavFlight/inav/pull/8403#issuecomment-1256169641, or unsubscribe https://github.com/notifications/unsubscribe-auth/AAHMBG7J5X6ZXVPKFLV4AMDV7WRPXANCNFSM6AAAAAAQPJSTUM . You are receiving this because you were mentioned.Message ID: @.***>

[b14ckyy]

So a few of our test pilots including me will do some "final" functional tests today/tomorrow. For me likely tomorrow unless the weather clears up a bit today. Shota and I are checking out 2 options to get rid of the remaining drift after some aerobatics, especially successive stop and go rolls maneuvers. First by checking what compensation method works better and if that does not help then likely removing ROLL from the ignore rate fallback as it was done in 3.0 and worked since.

After that I think I will post a beta build in the Fixed wing group and let a broader number of users test it in the field to get feedback. We had never any major issue for months now so I think it is safe for public testing.

[OptimusTi]

Test pitch!!!

[b14ckyy]

@OptimusTi Pitch is fine so far. Not seen any issues there.

[OptimusTi]

@OptimusTi Pitch is fine so far. Not seen any issues there. From what I have seen in videos looks delayed and not keeping track compared to roll

[b14ckyy]

@OptimusTi there where a lot of videos and not all of them are with this PR but https://github.com/iNavFlight/inav/pull/8407 On the other one yes there is an issue with vertical acceleration Julio could not fix yet, causing pitch issues. This is not a problem with This PR here.

---

More testing done today in some good winds. I know where this stepped roll drift comes from. it's actually my inputs. every time I stop I correct the pitch angle and then do the next roll. but every time I do this I carry some pitch into the roll. this causes 3-4Gs every time I start a roll. when I do stepped rolls with no pitch input there is no problem. This causes some drift to accumulate. IMHO we can ignore this as usually no one flies this way and it also looks crap when doing aerobatics. Not even sure how we could counteract this. I think we shouldn't.

Also tested with no GPS activated. AHI drift is there but it is controllable and never exceeds Angle limits for Angle mode. As the drift compensation uses Reference airspeed if no GPS is present, this parameter should be in the configurator GUI so pilots can set it based on the average maneuver speed of their LOS planes if they have no GPS, this should minimize the drift.

I saw not a bug difference between VELNED and TURNRATE mode when cruising but I have a feeling that in sharper turns or more aggressive flying, TURNRATE/ADAPTIVE is a little better.

From a fixed wing perspective and in my opinion this is ready to merge with ADAPTIVE Method as default, 15/10 ignore rate/slope and 2000/50 kp/ki. The only thing I recommend is, to keep roll out of the ignore-rate mitigation. I tested with and without roll rate mitigation today and saw no downside in not counting roll but for non-GPS planes that actually use ignore_rate at the set values, this will become important. Especially for slope gliders who fly in strong winds having their planes rocked around. It was this exact reason why roll was removed from the ignore rates in 3.0 already.

[b14ckyy]

Here are the latest test flights. One of the tests with GPS enabled. Right after launch I do this strange maneuver with Aileron rolls and pitch correction after each roll that causes some good drift. But beside that it is rock solid. Never more than roughly 5° of error after some turns. https://youtu.be/edKjR93SS7c

And this is a test with no GPS enabled. G-Force compensation only based on reference airspeed, that was set completely wrong anyway. It was on default 15m/s but should have been 20-24m/s for this plane. https://youtu.be/6qwS4-LOrtY

Both are very solid and imho totally reliable and safe to fly.

[DzikuVx]

@shota3527 @b14ckyy I'd like to merge it to master already and if there are any future improvements do them in separate merge requests. Any objections?

[b14ckyy]

@b14ckyy We are discussing a few last details right now just to make sure it is in a fully usable state for your beta release. After that in my opinion it is ready to merge. I agree we can do optimizations later, plenty of time.

[b14ckyy]

@DzikuVx Ready :)

[DzikuVx]

Awesom! Thanks a lot @shota3527 for pulling that off! Truly amazing work

[P-I-Engineer]

good defaults yet? currently they are at

imu_dcm_kp = 2000 imu_dcm_ki = 50

imu_acc_ignore_rate = 15 imu_acc_ignore_slope = 5

imu_inertia_comp_method = VELNED

according to @b14ckyy post should comp method be ADAPTIVE?

configurator defaults are changing these too on defaults selection

thanks