Improved Wind/Position EKF

I posted this earlier but did some tweaking on it to compare the Extended Kalman to my Fixed Gain Observer filters. I haven’t finished the FGO position filter but am really starting to lean more on EKF because the performance really is significantly better.

The idea for this filter is to fill in the gaps of the GPS position between the 1-4 Hz range for something like a quadrotor or just overall improved lag free navigation. You can clearly see how the fusion of more sensors can really increase your UAV's perception of reality!

**Edit, I improperly emulated GPS in the first pic so I added the correct pic. This shows how GPS comes in at 2Hz with noise and using a little logic and deadreckoning you can not only fill in the gaps but be more accurate than GPS is actually reporting. The GPS acts kinda like the accelerometers in the DCM algorithm, they keep the solution constrained.



E-mail me when people leave their comments –

You need to be a member of diydrones to add comments!

Join diydrones


  • Developer
  • Hi,
    good result!

    Did you used this equation :

  • Developer
    I currently only have heading/velocity + wind written. But yeah that was the whole idea of the quick project. Now that it is running at least, I'm going to start adding noise to the sensors and put it on a more "varying" realtime sim. This was just a sanity check to see if I had all of the math right. I'm swamped with work right now but when i get both methods (acc and vel) finished with some realistic noise on sensors I will publish the code.

    Stay tuned!
  • Developer
    Yeah I would like to see the source code also. The GPS estimation accuracy is impressive.
  • Are you using any accelerometer/gyro information to propogate your position estimate, or are you just using the previous heading/velocity + wind information?
    How well does your algorithm work in varying winds?
  • Any chance of publishing the algorithms? Looks good so far :D
This reply was deleted.