NAV - Use model-based estimator for navigation (position/altitude/velocity) about inav HOT 39 CLOSED

There is a work-in-progress on inertial predictor/corrector model-based estimator. Soon it might replace the currently used estimator.

from inav.

Tests of new estimator in althold mode are good. Poshold test failed due to some stupid error in 1 line of code. New tests pending.

from inav.

@digitalentity I was wondering if it is possible to use position hold without GPS indoor, actually using a position estimator should also predict according to the status of the system where the multirotor is. Normally to keep position it should be necessary and optical flow sensor, but I think that the estimator should be enough to stabilize little drift in hoovering when inside home, do you think you can make a mod to enable it, I wish I could try outside with gps but in Florence it's heavily raining, so I'm limited to indoor tests

from inav.

the idea would be to compensate acceleration higher than a certain threshold and not touch the rest in order to avoid too much saturation on the PIDs

from inav.

from inav.

let me know when you do it and in which branch I'll give it a try as well

from inav.

@digitalentity just some small details: the position hold should be more a stabilization system similar to the angle/horizion way of navigation, so if sticks are in middle position it try to stabilize, if the sticks are outside middle position sticks win over it

from inav.

@bk79 Just coded a quick and dirty horizontal position dead-reckoning. UNTESTED, not enough space in the house to launch a 450-sized machine. You can grab a 8b0af7404e04bd3f8463aaf451178e932d5aa23b and give it a try.

If you have a blackbox you can use it to verify dead reckoning is working. Launch, hover for a while, download the log and look at navPos[0..1] and navRealVel[0..1] variables. They are measured in cm and cm/s respectively and should both stay near zero while hovering when using dead reckoning.

Please also note that if you have a compass, it should be calibrated.

from inav.

@bk79 disregard previous commit, use this one: 1d1b65825e7b646ef2132421d645d6c208006168

from inav.

@digitalentity unluckily cc3d is in the quad without motors (I'm still waiting for banggood to deliver them) so no flight log, but on the hexa with sparky I can test if it works in a few minute and let you know, did you create a separate mode for that or is the same for PH?

from inav.

That's the same PH mode but allowed to work without GPS, using only
accelerometers. Not sure it will though :)

On Sunday, October 4, 2015, bk79 [email protected] wrote:

@digitalentity https://github.com/digitalentity unluckily cc3d is in
the quad without motors (I'm still waiting for banggood to deliver them) so
no flight log, but on the hexa with sparky I can test if it works in a few
minute and let you know, did you create a separate mode for that or is the
same for PH?

—
Reply to this email directly or view it on GitHub
https://github.com/digitalentity/cleanflight/issues/23#issuecomment-145328511
.

Sent from Gmail Mobile

from inav.

@bk79
Just did a simple hovering test in a room. Didn't risk to enable poshold as room has only 1x2m free space. Estimated position looks very close to what it actually was. I think new estimator is doing it's job rather well. Blackbox log is attached. Please note how well navPos[0] and navPos[1] follow the roll/pitch stick.

from inav.

@digitalentity I've had a quick flight I tested with default setting and also changing pids to see if it was improving something, this is the feeling: if I enable it the control feels stiffer that means that it is doing something, but it keeps on drifting, I tried adding some derivative but looks like it tries to add more drift to it, my feeling is like the control is trying to keep the actual velocity more than the position. I'm wonding if working with a simple Proportional gain would make it better since whenever we hover we give just one small shot to the stick to stop it in position

from inav.

It should drift as accelerometer is very noisy. How much drift did you have? In my tests velocity estimation error was about 1-10 cm/s depending on tilt angle and vibration. Without a relatively good source of velocity/position (GPS, OpFlow) this is probably as good as it can get. I'll play around with it and see if I can make it any better.
You can try reducing INAV_W_XY_DR_P and INAV_W_XY_DR_V in the code here: https://github.com/cleanflight/cleanflight/blob/1d1b65825e7b646ef2132421d645d6c208006168/src/main/flight/navigation_rewrite_pos_estimator.c#L89
These two defines control how fast estimated position and velocity decay to zero when using dead reckoning, shouldn't be zero as this will lead to error accumulation. Try reducing them to i.e. 0.05 and see if it gets any better.

from inav.

@bk79
It just came to me - you are actually the very first one flying the new position estimator in poshold mode. I was unable to fly it due to bad weather. You didn't crash, that's good news! 😆

from inav.

the kind of drift I have is always in a direction once the PID has decided which is the direction it wants to go no matter what kind of correction I do as soon as I release the stick it goes back in the decided direction. consider that my loop is 1000us let me try first increasing P and removing I just to see that. One more thing wheneve PH is active I can decide to go down or up with motors like I would do with alt hold

from inav.

because I'm home without my daughter and my wife which would have made impossible any test, I stil have 10 days before the get back home from vacation

from inav.

@digitalentity ok AMAZING!!!!! it was just a matter of tuning PID for something like 15 20s seconds it didn't move AT ALL from its position, from what I see the PID is a little numb on constant velocity, but that's something expected since we're reading acceleration and if speed=constant acc=0, so probably it would be better to increase the sensitivity of the controller, secondly I think it's better to separate this PH from the PH with GPS, I'm not sure that the PIDs would be good for boths and adjusting one would dirsrupt the behaviour of the other. those are my actual value for PH on PID4:
P=14 I=0 D=0.050

from inav.

btw I'm hoovering over the bed so 200x200 without getting out of it and with a lot of noise due to the airflows of the 5030 3 leaf blades (6 motors)

from inav.

@bk79 Thanks for testing this, indeed result is amazing!!! This means GPS-aware mode will work as well and probably without much change in PIDs as GPS-aware position estimation is essentially the same dead-reckoning and GPS is used only for slowly correcting accelerometer drift. Making this a separate mode is a good idea, thanks!

from inav.

no problem just one thing, it would be better to correct ph behaviour when changing throttle position so to match alt hold mode, it's not nice when you desperately try to lower the copter and it stay sticky there

from inav.

@digitalentity I had more tests, the DR works good only if accelerometer is perfectly trimmed looks like try to keep an angle more than speed/acceleration change

from inav.

Yes, dead-reckoning is heavily dependent on accelerometer trim and attitude estimation. If PosR I-term is zero poshold is unable to compensate for imperfect accel trim or external disturbances like wind.

from inav.

@digitalentity I'll have another try with I but it was barely controllable with I enabled

from inav.

Next to impossible to debug this without blackbox :(
Hopefully weather will be ok in a few days and I finally will have a chance to record a few logs and actually get to debugging things.

from inav.

@digitalentity I had a try using acro mode, whenever I set the PH (DR) it looses heigh going down to the ground, something that doesn't happen when in level mode

from inav.

New, faster and more precise IMU is being developed in navigation-rewrite-quaternion-imu, it will improve performance of INAV. Automatic accelerometer bias offsetting is also in the works.

from inav.

@digitalentity you mean that the accelerometer will automatically trim itself during flight?

from inav.

For the purpose of velocity estimation - yes, but only if some reference is
present (baro/gps). Might not work in presence of sonar (need testing).
For the purpose of self leveling (angle mode) - no, not possible as we
don't have a reference for that inflight.

Sent from Gmail Mobile

from inav.

@digitalentity once you get acceleration=0 your accelerometer is actually reading constant speed (really almost impossible case) or zero speed which means self leveling is ok or drifting constantly

from inav.

The problem is that accelerometer sometimes does not read zero acceleration when at constant speed. This might be caused by slight misalignment or hardware bias. 5-15 cm/s/s is not a big deal for IMU self leveling (its only about 1.5% of gravity), but its a problem for velocity estimation - velocity error will be growing in time.

Estimator is constantly compensating for this error from GPS and baro, but it would be good if it will be able to compensate for the very cause of velocity error (biased acceleration).

from inav.

We should also take into account fake acceleration given by angular
rotation and misalignment of accelerometer from center of rotation

On Fri, 16 Oct 2015 08:02 Konstantin Sharlaimov [email protected]
wrote:

The problem is that accelerometer sometimes does not read zero
acceleration when at constant speed. This might be caused by slight
misalignment or hardware bias. 5-15 cm/s/s is not a big deal for IMU self
leveling (its only about 1.5% of gravity), but its a problem for velocity
estimation - velocity error will be growing in time.

Estimator is constantly compensating for this error from GPS and baro, but
it would be good if it will be able to compensate for the very cause of
velocity error (biased acceleration).

—
Reply to this email directly or view it on GitHub
https://github.com/digitalentity/cleanflight/issues/23#issuecomment-148617826
.

from inav.

@MauroMombelli True, but virtually impossible to compensate for :(

from inav.

not if you let the user input the distance; by default we can use 0,0,0 but user may set up it. Then given the angular rotation estimated by the difference between the old and new quaternion (using quaternion instead of gyro should give us filtered and qualitative better result)

i've done some calculation, and those acceleration can be quite impressive if out of center by 2cm.

Also calculating this distance is not so hard and people may share it like i've seen for engine position for mixer

from inav.

@MauroMombelli
At a quite typical rotation rate of 100 deg/s (1.74 rad/s) a 5cm displacement gives us 15 cm/s/s error in measured acceleration. So yes, this error is worth compensating for. We might also want to consider compensating not only for angular rate but also for angular acceleration.

However, calculating the center of rotation is tricky. I'll think about a possible way to automatically calibrate this (possibly even in-flight).

from inav.

at least for now i would go on the fast way, calculate the baricenter in
real life and measure the distance; this is quite easy to do.

Then if you want to add some autotune is fine, but it goes out of spec (and
btw using magnetometer for pitch/roll would help smooth this kind of error)

2015-10-16 14:58 GMT+02:00 Konstantin Sharlaimov [email protected]:

@MauroMombelli https://github.com/MauroMombelli
At a quite typical rotation rate of 100 deg/s (1.74 rad/s) a 5cm
displacement gives us 15 cm/s/s error in measured acceleration. So yes,
this error is worth compensating for. We might also want to consider
compensating not only for angular rate but also for angular acceleration.

However, calculating the center of rotation is tricky. I'll think about a
possible way to automatically calibrate this (possibly even in-flight).

—
Reply to this email directly or view it on GitHub
https://github.com/digitalentity/cleanflight/issues/23#issuecomment-148710188
.

from inav.

Correct me if I'm wrong:
gyro X-axis rotation is roll rate - affects Y and Z acceleration
gyro Y-axis rotation is pitch rate - affects X and Z acceleration
gyro Z-axis rotation is yaw rate - affectr X and Y accel.

So basically I do the following:
accX += gyroY * distX + gyroZ * distX;
accY += gyroX * distY + gyroZ * distY;
accZ += gyroX * distZ + gyroY * distZ;

Here:
accX, accY, accZ is measured acceleration in body frame (cm/s/s);
gyroX, gyroY, gyroZ - rotation rate (rad/s)
distX, distY, distZ - accelerometer displacelemt from barycenter (cm)

from inav.

@MauroMombelli
Have a look at 2d7629d . It's basically a stub, next step will be to make this working and configurable.

from inav.

This seems to be working. Some regressions are still there, but now inertial position estimator is used for navigation and this issue can be closed.

from inav.