Search code examples
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:

    • If state vector does not include orientation, pre-transform optical flow speed to world coordinates, and then use a linear observation model (zero matrix with ones in the position of X, Y speed in your state vector)
    • If state vector includes orientation, you can write the full observation model that transform between world-body coordinates using angles.

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