Opticflow velocity direct to stabilization control loop

[knmcguire]

In paparazzi, the optical flow module calculates the estimated velocity with the bottom camera and sends this to a special guidance hover module with a separate controller. However I don't think this fits within the structure of paparazzi.

Wouldn't it be better to send the velocity estimate to the general Kalman filter, so the lower, stabilization control loop can handle the hover part? I think this would really improve the performance, as it will be combined with reading of the other sensors (IMU and such) and only level of control needs to be tuned.

[gautierhattenberger]

Sure, we need to do something like that. The opticflow was experimental and we now need to properly publish its data to the system so that filter can use it if needed.
Help is always welcome :)

[knmcguire]

I will try to look into it too. I know how the optical flow module is structured however I have to look how on for instance how I can send these values to the filter the right way. Might need a bit of help with that.

[gautierhattenberger]

The internal communication should use http://wiki.paparazziuav.org/wiki/Abi. You can also have a look at the global architecture http://wiki.paparazziuav.org/wiki/DevGuide/DesignOverview#Airborne_Functional_Diagram.
The AHRS/INS are already using the mechanism to get IMU and GPS data.

[flixr]

In master the OPTICAL_FLOW and VELOCITY_ESTIMATE ABI messages are already published (see #1350).
So basically subscribe to the VELOCITY_ESTIMATE message in the filter.... maybe we need to include some sort of confidence/covariance fields as well...

[flixr]

@knmcguire are you working on integrating the velocity estimate from the optic flow into the INS?
Need any help?

[knmcguire]

Hey thanks for asking. I havent had time to look into it due to preparing for a deadline. I have quickly looked in the hf_float.c file, but I couldnt find a way yet to add this to the measurement updates and where to base the noise model on. If you have suggestions about it, it's of course very welcome. But next week, when I can really focus on it, I will probably ask some questions here about it:)

Verstuurd vanaf mijn iPhone

Op 4 nov. 2015 om 10:11 heeft Felix Ruess <notifications@github.commailto:notifications@github.com> het volgende geschreven:

@knmcguirehttps://github.com/knmcguire are you working on integrating the velocity estimate from the optic flow into the INS?
Need any help?

—
Reply to this email directly or view it on GitHubhttps://github.com//issues/1387#issuecomment-153647579.

[flixr]

I quickly added that in the ins_velocity_estimate branch (commit 9a88ab5).
Something like that should work, the measurement noise is a define for now, although it would be better to compute some covariance estimate in in the opticflow calculator and send that with the VELOCITY_ESTIMATE.

Also if you wanted to rewrite the hf (horizontal filter), please go ahead ;-) It started as a very quick solution before a competition and is not really how such a filter should look like (e.g. there are essentially two separate filter for x and y).
Or a new INS written in float...

[knmcguire]

thanks:) I just fetched the branch and looking at it right now. Probably I'll make some updates on it soon

[knmcguire]

So, an update of the progress. I changed some things to your commit. I had to rotate from camera frame to body frame, and convert it from cm/s to m/s. After adding a simple quality measure to it, I was able to make it hover in AP_mode_hover_z_hold, with Horiz_loop: use_ref=false (or max_vel on 0.3). With this, and not even have to adjust the gains in the horizontal controller, it compensated for velocity drifts really nicely. I would say it performed even better than the optical flow module (that is with using an PID controller and no filtered velocity).

Anyway I don't have permission to upload this to the original branch. Should I pull request it to that branch or leave it on my account?

[flixr]

If you have a clean solution you can directly make a pull request to merge it into master, otherwise plz make a pull request to merge it into the ins_velocity_estimate branch

[flixr]

Closed with #1412