Blogs

Developer

Position & Wind Extended Kalman

Posted by Ryan Beall on February 7, 2010 at 10:00pm

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

airspeed

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 –
Follow

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

Join diydrones

Comments

  • Developer
    Ryan Beall February 8, 2010 at 7:06pm
    Yes, you simply "predict" more than you "update". Just keep looping the prediction model until a new GPS fix comes in. Realize that there is lag in the measurement also so....

    As for Covariance. In practice I never calculate aka estimate the covariance. I always just tune the numbers in practice (or simulation if you have it). That will get you really close. I suck at statistics but I can fiddle with numbers and get it working in about 3 seconds.
  • Daniel Clarke February 8, 2010 at 5:36pm
    Thankyou for such a quick response. Does this mean you use the previous predicted state as the previous state in the state prediction equation? Ie: x'(k) = f(x(k-1), u(k-1)) would become x'(k) = f(x'(k-1), u(k-1)) between measurements? (Where x' is the predicted state). Also, how do you turn an accuracy, of say 5m (the accuracy of my GPS), into a variance? Is the accuracy directly the variance? Again, thankyou for your help.
  • Developer
    Ryan Beall February 8, 2010 at 4:29pm
    both are correct, it depends on implementation. In this case I trust the GPS position a lot and it's only purpose in the model is to give me winds. In Short I don't let the EKF move the Position of the aircraft all that much but let the EKF have full reign over the winds. Also you can break up the EKF in half and just run the first half until you get new measurements thus doing what you described. Its all about what you want out of it
  • Daniel Clarke February 8, 2010 at 4:25pm
    I have a question a little off topic regarding the Kalman filter. I've just implemented one (EKF) for a ground based vehicle. My initial understanding was that an EKF was somewhat similar to a state predictor could give you estimates of the vehicle's state in between sensor readings. Ie, the GPS updates at 1Hz, but the EKF could give estimates at multiple Hz? But now I am of the understanding that the EKF gives you data of greater integrity at the same rate as the sensors would give; ie only as fast as your slowest sensor. Which is correct?
  • T3
    William Premerlani February 8, 2010 at 4:21pm
    Hi Ryan,

    I agree with you. The problem of estimating the wind is easy if you have an airspeed sensor, and it is trivial if you have a magnetometer, too.

    We do not have either a magnetometer or an airspeed sensor on the UAV DevBoard, so the focus of windspeed estimation for us is to first estimate the airspeed. We need the plane to be making some pitching or yawing movements to do that, anything but a straight line. As soon as we have an airspeed estimate, we can then estimate wind speed. We do some smoothing of the wind speed estimates, so it takes a few turns to get initialized. But once we have a good estimate of the wind, we then can make continual estimates of the airspeed, even when traveling in a straight line, by subtracting the wind vector from the ground speed vector. An occasional turn provides updates for our wind estimate.

    Best regards,
    Bill
  • Developer
    Ryan Beall February 8, 2010 at 3:30pm
    Yeah it is overkill, but I just wanted to see if I could matlab it up. The real reason it sparked my intrest is because even though it is a full EKF, a lot of the nonlinearity drops out in all of the zeros. So I just wanted to see how the full working model worked and then see if it was worth doing all of the stream lining. It probably is fairly easy to take all of the matrix math out beings everything is mostly zeros. However with that being said, I'm already working on a much more streamlined observation filter sort of like you described. Instead of being low passed, it is smoothed and has an integral affect to it. Kind of hard to describe but I will write it up soon. Much easier! I just like the EKF because of its elegance!
  • T3
    William Premerlani February 8, 2010 at 3:19pm
    Hi Ryan,

    Nice work.

    Regarding Paul Bizard's comment, the UAV DevBoard team has developed and successfully tested a 3D wind estimator that works without an airspeed sensor, magnetometer, or true heading. Inputs are GPS course and speed over ground, and the direction cosine matrix.

    In our case, we developed a DCM based algorithm, because the UAV DevBoard takes a minimalist approach, it does not have an airspeed sensor or a magnetometer. All we have are accelerometers, gyros, and a GPS. So, we squeeze everything we can out of the direction cosines.

    We estimate all three components of the wind, including the Z (or lift) component, which will be helpful for sailplane pilots.

    Here is a link to the theory.

    Here is a cross link to our posting.

    Best regards,
    Bill
  • VoiceOfReason February 8, 2010 at 2:19pm
    Good stuff Ryan...even with magnetometers, wind estimation is important for better path following and auto landings.

    Since you have all this information however, is the EKF possibly a little overkill? Since most GPS give you NED velocity (Ublox does at least), then the velocity of the plane in NE space is:

    Vplane,N = Vair,N + Vwind,N
    or
    Vplane,N = Vair*cos(psi)cos(theta) + Vwind,N

    Likewise,

    Vplane,E = Vair*sin(psi)cos(theta) + Vwind,E

    You know Vplane,N and Vplane,E from your GPS NED output velocity.
    You know Vair from some pitot tube or lookup table against throttle
    You know psi from your magnetometer (assume know pitch from IMU)

    The only remaining unknowns are Vwind N, E, which are just solved as:
    Vwind,N = Vplane,N - Vair*cos(psi)cos(theta)
    Vwind,E = Vplane,E - Vair*sin(psi)cos(theta)

    You can filter these with a low pass if they are too noisy, but that would be less computationally taxing then the EKF.

    Or, I could've missed something...thanks for posting the matlab code BTW...
  • Developer
    Ryan Beall February 8, 2010 at 10:57am
    Here is the stripped out matlab code. Got to love matlab! Its awesome for matrix math!

    wind EKF.txt
  • Developer
    Ryan Beall February 8, 2010 at 6:59am
    Yeah multiple states would be fun to work on but impractical on an 8bit processor. Need something like a blackfin.
This reply was deleted.