kalman-filteropticalflowsensor-fusion

How to use the extended kalman filter for IMU and Optical Flow sensor fusion?


I am building a quadcopter and i am using the pixhawk autopilot system with the px4flow sensor attached for optical flow data. The px4flow is a high speed smart camera (arm processor) with integrated gyro and height sensor, and it outputs linear velocities from the internal optical flow algorithm.

Now, i would like to improve on my position and velocity estimates by using an extended kalman filter to fuse the IMU and optical flow data. I have already derived the state model function and the state transition matrix for the prediction step.

By problem lies within deriving a measurement model/function for the optical flow velocities, to be used in the update phase of the extended kalman filter. I believe i have to derive it from the optical flow algorithm some how, but that is as far as i have gotten.

*edit: Here is an article describing the px4flow unit and how it calculates the velocities. (forgot to add the link, now it's there)

https://pixhawk.org/_media/modules/px4flow_paper.pdf


Solution

  • I'll assume (since you don't give any details) that state vector contains speed and position in world coordinates. Optical flow returns XY speed in body coordinates, so you have to rotate it according to yaw angle to get speed in world coordinates.

    If this is your case, you can choose between:

    That's basically the process. However, to get a better answer you should provide more details.