pose_estimator_inav - get's flow validation just one

[TSC21]

Hi,

I have detected that in the current implementation of pose_estimator_inav, I get a positive flow validation just once (the first one). Then, after losing that validation, I'm not able to get it again. I already tried to also add the proper brackets here, with no effect.

@DrTon can you please help to figure out why does inav just get's a flow valid once? My quality values are stable and above the threshold of validation. Just when it drops to zero (and then flow timewout) and then I get it again to good values, the system doesn't receive valid again.

By the way, I don't seem to get the sonar into the estimation properly. Please, if you may, check https://groups.google.com/forum/#!topic/px4users/3lxTOz_-GmE. The idea is to cancel baro and just use sonar+vision altitudes.

Thanks in advance!

[TSC21]

Regarding the second issue: In https://github.com/PX4/Firmware/blob/stable/src/modules/position_estimator_inav/position_estimator_inav_main.c#L956, doesn't sonar also enters in the inertial filter? Where does sonar measurements effectively enters in the altitude estimation?

Update: after some testing around the weights of each sensor, I came to the conclusion that sonar is used only to correct the z measurement as an offset of baro/gps heights. Is that right? So getting a 0.0 value of weight to baro will mean a none value of z measurement to the estimator, and this one will constantly predict a correction towards sonar offset, which means a infinite increase/decrease of predicted height.

That means that, for inav, or we get a correct compensation between what we increase in sonar weight and what we decrease in baro weight, or we have to change the way the estimation filter is structured, so to add sonar as a primary z value of estimation.

[DrTon]

Where does sonar measurements effectively enters in the altitude estimation?

Nowhere, distance to ground estimated separately, because distance to ground != altitude, and even distance to ground != altitude + const. Using sonar for altitude estimation creates a lot of unresolvable and dangerous problems. In position controller using distance to ground can be easyly implemented if needed.

[TSC21]

In position controller using distance to ground can be easyly implemented if needed.

Is that the best approach in this case? Can you add it so it can stick to distance to ground instead of altitude. Cause, for indoor cases, altitude it's not needed. (and thanks for clarifying that difference :+1: )

[DrTon]

The idea is to cancel baro and just use sonar+vision altitudes.

I think it's very special lab setup and it would be easier to write separate position estimator. You will not have to do with fusion with GPS and baro and code will be much cleaner. PX4 design allows to replace estimators very easily, simply use position_estimator_inav as template and drop everything you don't need.

[TSC21]

PX4 design allows to replace estimators very easily, simply use position_estimator_inav as template and drop everything you don't need.

Problem is I don't know what to add/remove from the code :S The thing is this implementation can be useful for the case we want to proceed from a indoor to outdoor environments, and vice-versa. So getting out GPS and baro doesn't seem to be the best approach.

What do you recommend to do with the current implementation, so that the baro estimate almost isn't considered? Maybe get vision_z with a higher weight?

[TSC21]

By the way, did you checkout about the valid flow stuff?

[DrTon]

Problem is I don't know what to add/remove from the code :S The thing is this implementation can be useful for the case we want to proceed from a indoor to outdoor environments, and vice-versa. So getting out GPS and baro doesn't seem to be the best approach.

Better approach for doind this would be Kalman filter, but it will be much more complex.

What do you recommend to do with the current implementation, so that the baro estimate almost isn't considered? Maybe get vision_z with a higher weight?

There is baro offset estimation, i.e. offset between barometric and true altitude. Now "true" is GPS altitude (very noisy), in your case it will be vizion_z. What you need is to set baro offset to difference between vizion_z and baro, so baro innovation will be always 0. In indoor environment baro is indeed very noisy and shouldn't be used at all if you have vision estimate.

Anyway it's not so simple and you will need some expiriments. If you are ready to start, feel free to contact me in skype (current76) or gtalk (rk3dov) and we will think online how to do this.

[DrTon]

By the way, did you checkout about the valid flow stuff?

Unfortunately I have no vision system here and only can help remotely, so need to look your log at least.

[TSC21]

Unfortunately I have no vision system here and only can help remotely, so need to look your log at least.

This is not happening with vision estimate being sent. It's happening in general! So this is something that's happening with just px4flow connected.

Better approach for doind this would be Kalman filter, but it will be much more complex.

Yeah, that's why, for now, is off topic to me 'cause this is for my MSc project and I don't have much time left to do more coding and testing. Need to adapt to my needs what already exists, since what already exists seems good and just needs a little tweak.

What you need is to set baro offset to difference between vizion_z and baro, so baro innovation will be always 0.

Can you point out that on the code so I can take a look?

In indoor environment baro is indeed very noisy and shouldn't be used at all if you have vision estimate.

That's why I would like to tweak so that sonar can have a more proper weight on height measurements.

If you are ready to start, feel free to contact me in skype (current76) or gtalk (rk3dov) and we will think online how to do this.

I will do it then :) Not today though. What's your availability tomorrow? In the mean time, you can make a list of what should be modified. The idea is not substitute the current pose_estimator_inav, but to improve it so it can be usable outdoors and indoors (that's why it's called INav right? :D ). Probably now, for my case, doing some code tweaking on the existing code is better than a new implementation of inav.

But I was thinking that maybe we could use quality variables for GPS, Baro and Vision depending on the quality of the signal, i.e. it's noise and precision. That way, the estimator will be dynamic and adjust the quality depending on the current status and noise of the sensor. Like, if GPS signal is lost, dropdown weight/quality of GPS and increase of Vision if it exists. Same thing with Baro.

For now what I want to be feasible is just use Vision as the main source of Z, px4flow as a combined source with Vision for X/Y and use the sonar to correct Z also. Not more than a one day implementation with testing (no props, just with device handheld). This is one of the main parts of my work so I totally need some help here :)

What do you think?

Thanks!

[TSC21]

@DrTon already sent you an invite in Skype! (nuno DOT marques DOT 163). When you have free time please send a PM so we can discuss what can be done on inav. Have to get this working soon.

Thanks in advance!

[t0ni0]

Hi @TSC21,

I have implemented sonar + baro altitude in position_estimator_inav in a branch of mine for indoor flight purposes. You can find it at https://github.com/elikos/Firmware/tree/sonar_alt. The main challenge when using sonar only altitude is, as you know, that it is noisy and works in a limited range. However, I have found that with the built-in spike filter in inav (by @DrTon), you can achieve good results for altitude estimation indoors with very small modifications. What I've done is not meant to be perfect and is actually a bit hacky by parts but it works well for me. I plan to improve it later, but no ETA on that. Here is a quick overview of what I have done so far:

• Limit the usable sonar measurements to 3m (originally 4m). I've had problems while approaching 3m of height because the flawed (error) measurements often fall into the 3.5m - 5m range, and then they "escape" the spike filter and get accounted in the estimation.
• Added INAV_SONAR_ZMIN parameter which determines the minimum (averaged) sonar reading for when sonar should be used for altitude estimation. This parameter is separate from distance used for flow velocity calculation which is hardcoded to a 0.3m - 3.0m range (originally 0.3m - 4.0m). You should never set this param under 0.3 because the sonar measurements never go under 0.3m. I left it by default to 0.5m because I found measurements from 0.3m to 0.4m to be a bit noisy.
• Added INAV_FLOW_Z_OFF parameter which is the distance from the bottom of your robot to the flow board. This is used to offset the altitude measurement so that the distance from the bottom of your copter to the ground is your altitude estimate and is separate from the flow to ground distance.
• Baro offset correction. When the copter is landed, the baro offset is constantly reset so that baro drift is irrelevant until takeoff. When taking off, the barometer is used until its weight is linearly blended with the sonar's from 0.3m to INAV_SONAR_ZMIN. When fully using sonar, the baro offset is again corrected using the sonar measurement to try to minimize the offset between baro and sonar estimates. This is so that when transitioning from using sonar to baro and vice-versa (going in and out from the valid sonar range), there are minimal or no jumps in the altitude estimates. I did not do the blending between sonar and baro above 3.0m because I personally didn't need to go higher, but it should work if you don't go higher than 3.0m for too long.

I did all this baro stuff because I had no choice but to use it for altitudes from 0m to 0.5m where the sonar would drastically fail, but I know it is very far from perfect. I also cut out the surface offset correction when using sonar because it would cause problems with the flow velocity estimates if the surface offset was modified and I was personally working with flat surfaces only. If you fly into a different level surface, the copter will simply follow and keep its current distance from the ground.

Otherwise if your PX4Flow or its sonar fails or you don't use it (connect it), the position estimator will work as usual except that your maximum limit would be 3.0m for flow to be valid. You can disable using sonar as height measurement by setting the INAV_SONAR_ZMIN to a very high value such as 100000.0.

Finally, this branch is based on an outdated version of master (somewhere around april 2014 I believe) and I haven't tried merging it with current master yet, and I sure haven't tried any vision position estimate yet because I don't have a setup for that (but soon will). Hope you may find something useful!

Antonio

[DrTon]

Antonio, I see that you made a big work! Some comments about surface level estimation: it also needed when going from high altitude to "good for sonar" altitude. After long flying baro drift may be significant and you will have offset between estimated altitude and distance to ground, so my approach was simply reinitialize "surface level". If you removed "surface level", you will have jump in this situation.

[LorenzMeier]

Outdated.