NAV - Althold problems

[digitalentity]

Setup: use_midrc_for_althold=1, throttle input=1500, altitude drifts. Possible cause - insufficent I-term to compensate for thermals, algorithm flaw, barometer problems.

[digitalentity]

Increasing I term to 0.25 solved the problem for some degree. Need to shield the baro from sunlight and test more. Looks like default PIDs for ALTHOLD (#4) need to be revised.

[digitalentity]

Shielding baro from sunlight helped. Increasing I-term helps as well. However, I've encountered Engaging ALTHOLD when climbing results in drop in throttle and altitude. Scary. Should record ALTHOLD PID internals to blackbox and investigate.

[digitalentity]

With default baro_tab_size = 21 and baro_noise_lpf = 0.6 barometric altitude gets delayed too much - we can not trust it for velocity calculations. Data would be reliable only on prolonged (several seconds at least) ascent or descent.

[bk79]

using a kalman filters should probably help instead of using a lpf and we get velocty calculation for free

[digitalentity]

I am not a math guy, it you can help with kalman - please don't hesitate to do so :+1:

[bk79]

there is already an implentation for GPS on Kalma filter https://github.com/lacker/ikalman that's the starting point since it uses the same variables. I have only one concern do we have enough cpu and ram to run it? Kalman filter is very heavy in terms of calculation even if I can assure you that the result is AMAZING, I used it to perform tracking of a coax during my thesis long ago but it was a a laptop performing the filtering and sending command to the RC copter. Consider that we need to math on matrixes that means lot's of time spent to calculate the status of the system

[digitalentity]

Thanks for the link, I'll have a look at it. You are probably right, kalman may be too taxing on CPU. However if it will be sufficient to update it with i.e. 1/10 looptime calculations can be split up and executed one-by-one. This needs to be looked into some time. I'll open an issue on this one as well.

[bk79]

mmm...that's not the solution let me explain a little how it works: kalman filter has the status and the inputs of the system, according to the actual status and the actual input it will evaulate next status of the system, and the measure incoming will correct pulling the system to converge to the filtered status: in small terms Kalman evaluate where we are, according to the previous condition of speed and position, and will correct any drift in the calculation when new measure arrives, this means that we're not using anymore the real measure but an estimation coming out from the Status matrix. So evaluation of the status of the system should be done every cycle, and update as much as possible to avoid drifting too much

[digitalentity]

I understand, but taking a snapshot of sensor readings and doing calculations for several cycles can be a solutions for not having enough CPU power to do filter update every cycle. The only drwaback of this is that filter will be updated as less rate, but that's ok since we are limited to about 40Hz for baro and 5-10Hz for GPS. There is simply no point in updating Kalman filter at 400Hz (at 2500us looptime)

[bk79]

mmm...looking that side then we can think of an update at 40 or also 50 Hz that is the servo signal update not considering oneshot in that case it should not be time critical, what I'm worried about is more about the ram and it comes to my mind the fact that we should focus for now only on one model, bacause a multicopter is not an airplane in terms of variable dependencies (I'm thinking about altitude)

[digitalentity]

Yep, fixed wing support is another problem. However if we use Kalman only for sensor fusion to get accurate position/altitude/velocity information, we should be fine. Control logic will be different for multirotor and fixed wing, but for both models input data is position+altitude+velocity. If Kalman can provide that information at i.e. 40Hz rate we should be fine. RAM is not a big problem - currently we are using about 13K of available 20K. Kalman will fit in the remaining 7K for sure.

[digitalentity]

@bk79 What do you think of using Central Limit Theorem for fusing sensor data together (pos/alt/vel) https://en.wikipedia.org/wiki/Sensor_fusion#Example_sensor_fusion_calculations It's fairly simple to calculate even for small cpus and it can fuse together varying number of sensors. The only thing we should know is variances for different sensors, but that's faily easy to figure out. GPS is reporting PDOP values, for sonar, baro and accelerometer integration we'll have to figure that out by experiment.

[bk79]

do you have some logs of the readings of both sensors? if you have them we can calculate the sigmas that would actually mitigate noises quite a lot

[digitalentity]

Not yet, I've just stumbled upon CLT. Do you think it's worth trying instead of Kalman? It looks fairly easy to implement.

[bk79]

it's not a substitute for kalman but it will work better than an lpf on both, as long as we can calculate the sigma variance (that is actually measurement dependent=sensor dependent). It's nothing else than a normalization of the measure to average values, 90% of the IMU are standard so it could work

[digitalentity]

I'll get this implemented in a few days. Will try it out once I flight test 542a1bf3261a2f19666dd7e8d5e54d28b85e7c9e and 9cf2826ac88f5e839efd5301ae58e41be7148bb1 and close #14. Don't want to make lots of fixes to code and then struggle to figure out which one is failing :smiley:

[bk79]

if you can implement and log it without using it for flight, we can have a comparison between data

[digitalentity]

Will look into it tomorrow, thanks for the idea!

[bk79]

if you finish it in time I'll test it as well on my cc3d on the 260 class on the weekend

[digitalentity]

I still need to figure out variances for accelerometer and baro velocities. Will definitely take some time.

[digitalentity]

Some variances for velocity in cm/s: MPU60x0 accelerometer integration result without deadbanding: 10-30 MPU60x0 accelerometer integration result with acc_deadband=10: 0-10 MS5611 baro (baro_tab_size = 21, baro_noise_lpf = 0.600): 550 MS5611 baro (baro_tab_size = 7, baro_noise_lpf = 0.200): 8000

For baro_tab_size=21 and baro_noise_lpf=0.6 variance is rather low, but has a huge delay (feels like 0.5-1s). This is unacceptable for navigation so I have to reduce filtering and rely mostly on accelerometer integration.

These experimental results closely correlate to my complementary filter for ACC+BARO vertical velocity tuned by trial and error. My value is 0.998, derived from these values: (1/10)/(1/10 + 1/8000) = 0.99875. That's good news - it means I'm on the right track with CLT-filter for sensor fusion.

Variances for altitude (in cm): MS5611 baro (baro_tab_size = 21, baro_noise_lpf = 0.600): 300 MS5611 baro (baro_tab_size = 7, baro_noise_lpf = 0.200): 400

[bk79]

@digitalentity I had a session yesterday I don't know why you have so much noise on baro but my bmp180 looks nice not that much variance I've got the log of one decent flight (then I crashed a couple of times) am I looking at the wrong variables?.

[bk79]

@digitalentity I have thought a little about it, if baro is not trustworthy in terms of speed change, simply let's use it heavily filtered only for positioning reference, as for the speed I think that the imu would be much more reliable so let's focus on that for calculating the speed and let's give the baro only the task of telling where we are. The only thing of caring about using the accelerometer is to make sure the Z acceleration is is correctly calculated even when the quadcopter is tilted

[digitalentity]

@bk79 Had a chance to make a very short test-flight today. Forgot to re-tune PIDs after custom filtering was merged to master, so I got lots of vibration on motors. The log for you to see is here Baro-related parameters:

baro_tab_size = 21
baro_noise_lpf =  0.600
baro_cf_vel =  0.985
baro_cf_alt =  0.970

We can not rely only on IMU, accelerometer is very sensitive to vibration and if it gets saturated, integral (velocity) gets biased. This bias can only be removed by some reference velocity (i.e. baro altitude derivative). Today I was not able to get altitude hold working as good as before, likely due to lots and lots of vibration that messed up with accelerometer.

[bk79]

mmm... I had a look baro reading look similar to mine, but I've noticed that you're a little over weighted, your motors are almost always above the average, that would actually force not easy PID gains have you thought about increasing size of the propellers and have a try with that?

[digitalentity]

My 650-sized fpv quad has a total of about 5.5kg thrust and AUW of only 1.6k (hovers at about 30-40% throttle). The recorded log is from my test copter that is destined to crash alot, so I use the cheapest available parts. It hovers at about 50-60% throttle, that's good enough for trying things out without having to spend alot of money if something goes wrong (that's happen all the time) :-)

[bk79]

same to me... saturday session I had 3 props and one leg gone because gps issue...one the thing that should be corrected on gps.md is the NAV-SVINFO setting with 100Hz if you set it with a value higher than 2 it will get gps comm on and off with ubox 6m

[bk79]

@digitalentity I've seen your navDebug[3] doesn't change that much, if you look at my 40 second test looks like having a completely different behaviour

[digitalentity]

Yeah, I noticed. I guess that's due to this function https://github.com/digitalentity/cleanflight/blob/navigation-rewrite/src/main/flight/navigation_rewrite.c#L1617 being called on every gps update. My variance for GPS velocity there is a some random number and that actually ruins CLT calculations.

[bk79]

yse but that should also affect my recording and that's not happening...how come?

[digitalentity]

No ides yet. I'll temporary remove GPS-related code from CLT and try again.

[bk79]

what is the model and rate you're using on your gps?

[bk79]

@digitalentity any update the status of the altitude velocity calculation?

[digitalentity]

@bk79 Lots of changed to current code. Found a cause for altitude drop when enabling ALTHOLD. Broken something in POSHOLD in process. Going to flight test and investigate soon.

[digitalentity]

90c49c2b11179fa43a941e444a5966e965b6e645 solves the problem with altitude drop when enabling ALTHOLD mode. Drift is also mostly gone - new IMU works pretty good. The issue of PID tuning still remains, but that could be resolved as a part of #4. No bugs regarding ALTHOLD so far. Closing.