Position & Wind Extended Kalman

Wrote up a wind estimator today. It is based off of a simple model which requires very little inputs from sensors.

psi - true heading (not course over ground)

theta - pitch


GPS lat and long

It is a simple model. The wind model has no dynamics which helps simplify the solution. This asusmption is mostly true beings the wind direction and velocity doesn't change all that much for a given flight. The filter starts off by assuming the soltion is a zero wind scenario and then moves towards reality. It takes about 2-3 seconds to converge.

The non-linear state update equations are as follows:

Pn_dot = Vair*cos(psi)*cos(theta) - Wn

Pe_dot = Vair*sin(psi)*cos(theta) - We

Wn_dot = 0

We_dot = 0

Notice the filter resolves on a wind velocity of 15 ft/s and 090 deg heading.

Pretty slick how quickly it finds a solution.

E-mail me when people leave their comments –

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

Join diydrones


  • hi,my name is shawngu,i work for a agriculture automation agency in china. basically,we provide automation solutions for billions of chinese farmers to  improve their farming efficiency. see,china is a big agricultural nation,so the profit in this area is really big.currently,our major interest is spraying crops by UAVs.and a lot of local government agencies are very intersted in our idea.however,there is no company in china currently capable of providing this kind of UAVs. lately,i came across the arducopter website and downloaded the source code.i figured the your copter has great potential to become the solution to our goal. we would like to invite you to be the product supplier and we want to act as an agent of your copter in china. if you are interested in this idea,please contact me .i'm really looking forward to hearing from you!

  • Developer
    I did some initial testing in the simulator today. The FGO (fixed gain observer) approach proved very acceptable for very little cpu crunching. It resolves on a solution very fast! Got to love Tim's Python/Xplane combo! Check out the sim truth value vs the estimated. They are close enough for me!

  • Developer
    I have done both methods. Now it is just easier to guess on the first flight. Pitch and roll are easy. Depending on how your control laws are set up normal "pilot rule of thumb" always are great starts. They teach us the 1-1 rule for heading. Ie if you are 10deg off of heading use 10 deg of roll to fix it. Well Guess what...if your heading controller is set up corectly that would be a Kp_heading = 1. Simple things like that that always hold true. And simple eyeballing the surface movements for a given ammount of error. The tricky one that usually takes me a couple of flights is airspeed and altitude hold. But none the less you can still sanity check all scenarios to make sure it will always find a safe condition ie: (hard code low/slow, high/slow, low/fast, high/fast) and see that everything makes sense. For this I just print out throttle and unplug the throttle so I have a good idea of what the controller is wanting to do.
  • Developer

    To get your turning right in one flight do you control the gains via telemetry uplink ?

    Or do you mean that your first guess on gains and downloaded into the firmware is usually correct by the second flight ?

  • Developer
    Yeah true true, It usually takes me about one flight to get my gains tuned? It would be nice to have something do it automatically but it just seems kind of unreliable and cumbersome
  • We could fiddle with the linearized airplane model in closed loop with the AP and establish the theoretical formulaes linking the AP gains to the airplane aerodynamic coefficients associated to a nice response, quick and without overshoot.
    We could then identify the airplane linearized aerodynamic coefficients CL,CD,... CnB,Cla,... by recording the reaction of the airplane to some normalized maneuvers (pitch then roll then yaw), and compute the gains.
    But this is a lot of work and given the modelization errors I'm not sure this is worth the pain.
  • Pete: I also went thru it rather quickly (at work .. bleh). One issue I saw with the MRAC scheme was that there is no guarantee of converging to a particular solution, rather - the the error is guaranteed to converge to zero. Perhaps this is the source of the oscillations. I do see this as an issue .. I don't think it would be wise to drive the servos with such rapid oscillations for extended missions.
  • Developer
    No problem guys....Adaptive control is not trivial by any means.
  • Developer
    Faisal, I speed read the 236 pages and focused on some of the summary.

    It seemed to me that the ailerons, elevator and rudder could still have high speed oscillations (even though the commanded roll, pitch and yaw was very good). Did you see that as an issue ? It was a major project. Exactly what I needed to see.

    Ryan, Thank you very much for posting that.
  • Ryan, thanks a bunch! Soooo .... anyone wanna implement MRAC D or L1 and try it out? Source code is given! I've been thinking about something like this for a while, some way to parameterize an airframe's dynamic response, and an adaptive way to learn it in-flight .... This may be what I've been looking for. I'm out of school ... no more free access to conference/journals :(
This reply was deleted.