Kalman Filter vs Extended Kalman Filter / Unscented Kalman f

Here is the place for all your dsPic or autopilot questions.

Moderator: lukasz

Kalman Filter vs Extended Kalman Filter / Unscented Kalman f

Postby asifjahmed » Thu Feb 18, 2010 1:49 pm

Hello -

I have been doing much research into the various flavors of Kalman filters. I have already successfully implemented a well-performing Kalman filter in c that runs on an arduino and fuses 3-axis accel and 2-axis gyro data to get absolute angles. However this does not work properly for all angles.

I have been reading that I need a nonlinear estimator for all angles, aka an Extended or Unscented kalman filter. However, it seems there is no sample source code available?

Any help is appreciated.

Thanks,

-Jamie
asifjahmed
 
Posts: 11
Joined: Mon Dec 14, 2009 6:27 pm

Re: Kalman Filter vs Extended Kalman Filter / Unscented Kalman f

Postby Mitch » Thu Feb 18, 2010 2:59 pm

I did extensive searching for several months for exactly that. There are multiple papers and thesis available that address it but I found no code - subsequently am writing my own. I have (almost) working mplab code for 3 gyros, 3 accelerometers, and a magnetometer triad with EKF using quaternions. The state vector is (q0,q1,q2,q3,pbias,qbias,rbias).

Right now I'm loosing the yaw axis update through +- 90 degrees - I haven't sorted that out yet. I expect due to a singularity or maybe loss of accuracy when the magnetometer axis aligns with the local magnetic field. Note - I'm still doing the update using errors from Tait Bryan (Euler) angles with dphi/dq, dtheta/dq, and dpsi/dq.

I am in the process of converting the error calculations to quaternions to eliminate some of the complications associated with the euler/quaternion conversions. Also converting to quaternions for the vector rotations for the accelerometer and magnetometer measurements.

My code is a work in progress (also not converted to gluonpilot yet - still using dspic33 with a sparkfun razor 6dof). Thus, I won't post it - but anyone who wants a working copy PM your email address and I will email the code.

MItch
User avatar
Mitch
 
Posts: 118
Joined: Sat Dec 05, 2009 1:59 pm
Location: Florida, USA

Re: Kalman Filter vs Extended Kalman Filter / Unscented Kalman f

Postby Tom » Fri Feb 19, 2010 2:46 pm

Yes that's also on my wish list.... some day when I have more time :-) I think it takes a lot of debugging before you get it right. Also, I don't really have an idea about the performance improvements of such a larger filter.

My preferred work of reference is this one: http://epubl.luth.se/1402-1617/2000/081 ... 081-SE.pdf
Because this thesis attempts to go "all the way" with a huge state matrix, it's interesting to see how far you can go. Also the basics and alternatives are explained really well.

Mitch, how are you planning to implement the update from accelerometer data? Directly as the kalman update of your 6x6 matrix?
I think this is the standard approach most current (kalman) implementations use. It would be interesting to simulate it using real data, and see how many variables are around 0 to optimize them out of the equations.

I plan to upload all papers I have collected over the years. Especially BYU has some interesting ones.
User avatar
Tom
Site Admin
 
Posts: 1016
Joined: Fri Nov 13, 2009 6:27 pm
Location: Belgium

Re: Kalman Filter vs Extended Kalman Filter / Unscented Kalman f

Postby Mitch » Fri Mar 05, 2010 4:55 am

I've written three different methods to incorporate the accelerometer and magnetometer data - simple trig (acceltoeuler + magnetometer data), vector cross products, and progressive quaternion rotations/vector matching with least squares error minimization. The latter is by far the most promising because all of the vector magnetometer data is implemented and the accelerometers are used to solve the rotation around the local magnetic field vector axis. The linearization is in the navigation frame so gps acceleration corrections can be easily integrated.

I so far have finished a Kalman (EKF) filter using quaternions and euler angles for the update. I am in the process of modifying the Kalman update to use the quaternion domain. I've started writing parallel code in matlab because it makes the debugging so much easier.

I agree with Tom's philosopy it won't be necessary for airplanes. My development is directed towards heli's, vtols, and platforms where static attitude data is required and the acceleration vector is less predictable. The update matrix will be a 6x6 but requires a determinate on only a 3x3. The euler update matrix is a 3x7 but requires a great deal of calculation for the conversions. I will definately have to do some code optimization once the basics are working.
User avatar
Mitch
 
Posts: 118
Joined: Sat Dec 05, 2009 1:59 pm
Location: Florida, USA

Re: Kalman Filter vs Extended Kalman Filter / Unscented Kalm

Postby arham » Thu Jan 29, 2015 11:41 am

Probably my question was not 100% clear... I already read the wiki and configured my plane as suggested.
My "autopilot behavior expectation" was that if I give on the RC transmitter full roll to the right, I expected the servo to turn full roll to the right until the Max rool =(eg the 45° of your post) is nearly reached...and then the PID takes the control over to maintain the roll on 45°....

Do I be wrong somewhere?
Lily lara
arham
 
Posts: 1
Joined: Thu Jan 29, 2015 11:28 am

Re: Kalman Filter vs Extended Kalman Filter / Unscented Kalm

Postby Tom » Sat Feb 07, 2015 7:46 pm

Yes, because the autopilot can respond so quickly to changes, the ailerons are only changed a little bit until the target roll angle is met.
User avatar
Tom
Site Admin
 
Posts: 1016
Joined: Fri Nov 13, 2009 6:27 pm
Location: Belgium


Return to Firmware

Who is online

Users browsing this forum: No registered users and 4 guests

cron