Hi, I've been undertaking a project to create a controller for a quadrotor aircraft. Whilst the dynamic modelling and controller design has nearly been completed I am having trouble getting the requried feedback signals. I am trying to implement a Kalman filter to combine the signals of accelerometers and gyroscopes to produce an attitude estimate. I have read all the introductory literature and have also obtained some code to achieve the desired sensor fusion along a single axis. The code I have is here: 124.243.132.204\tilt.c The code uses the derivative of the covariance matrix to do the update: Pdot = A*P + P*A' + Q The P matrix is then updated using P=P+Pdot*dt The introductory papers however all use the update equation: P=A*P*A'+Q Why is the derivative used and where does that equation come from? I havent been able to find anything on it. There doesnt seem to be a lot of info around on the net and Kalman filtering seems to be something that not many people have an understanding of. This is my main question but I am also wondering how the A matrix is derived. I can not see how the fusion of the gyro and accelerometer data can be achieved with this matrix when fitting it to the form x=Ax+Bu. I assume x would be [tilt angle, gyro bias] and u would be the gyro data. Anybody got any ideas at all?? I am completly lost and confused.

# Kalman Filter Help - Anybody understand Kalman filtering??

Started by ●October 1, 2006

Reply by ●October 1, 20062006-10-01

You might look at http://www.innovatia.com/software/papers/kalman.htm In article <TbWdnS50Sv9fJYLYnZ2dnUVZ_rqdnZ2d@giganews.com>, "quadrotor" <quadrotor@gmail.com> wrote:>Hi, > >I've been undertaking a project to create a controller for a quadrotor >aircraft. Whilst the dynamic modelling and controller design has nearly >been completed I am having trouble getting the requried feedback signals. > >I am trying to implement a Kalman filter to combine the signals of >accelerometers and gyroscopes to produce an attitude estimate. I have read >all the introductory literature and have also obtained some code to achieve >the desired sensor fusion along a single axis. > >The code I have is here: 124.243.132.204\tilt.c > >The code uses the derivative of the covariance matrix to do the update: >Pdot = A*P + P*A' + Q >The P matrix is then updated using P=P+Pdot*dt > >The introductory papers however all use the update equation: >P=A*P*A'+Q > >Why is the derivative used and where does that equation come from? I >havent been able to find anything on it. There doesnt seem to be a lot of >info around on the net and Kalman filtering seems to be something that not >many people have an understanding of. > >This is my main question but I am also wondering how the A matrix is >derived. I can not see how the fusion of the gyro and accelerometer data >can be achieved with this matrix when fitting it to the form x=Ax+Bu. I >assume x would be [tilt angle, gyro bias] and u would be the gyro data. > >Anybody got any ideas at all?? I am completly lost and confused. > > > > >

Reply by ●October 1, 20062006-10-01

quadrotor wrote:> I've been undertaking a project to create a controller for a > quadrotor aircraft. Whilst the dynamic modelling and controller > design has nearly been completed I am having trouble getting > the requried feedback signals.Filtering issues aside, your system is underdetermined. With accelerometers and rate gyros alone, it is impossible to distinguish sensor biases from real motion. You need some sort of absolute reference sensor to combine with your data. For example, a 3-D magnetometer would give you an attitude reference with no inherent drift, or a GPS receiver would give you a position and/or velocity reference with no drift. -- Dave Tweed

Reply by ●October 1, 20062006-10-01

quadrotor wrote:> Hi, > > I've been undertaking a project to create a controller for a quadrotor > aircraft. Whilst the dynamic modelling and controller design has nearly > been completed I am having trouble getting the requried feedback signals. > > I am trying to implement a Kalman filter to combine the signals of > accelerometers and gyroscopes to produce an attitude estimate. I have read > all the introductory literature and have also obtained some code to achieve > the desired sensor fusion along a single axis. > > The code I have is here: 124.243.132.204\tilt.c > > The code uses the derivative of the covariance matrix to do the update: > Pdot = A*P + P*A' + Q > The P matrix is then updated using P=P+Pdot*dt > > The introductory papers however all use the update equation: > P=A*P*A'+Q > > Why is the derivative used and where does that equation come from? I > havent been able to find anything on it.I assume they use the derivative because they feel they'll get some numerical advantage over doing the direct calculation. The expression for Pdot doesn't look right to me, however -- I'm not saying it _isn't_ right, but I'd have to dig into the math to verify it for myself.> There doesnt seem to be a lot of > info around on the net and Kalman filtering seems to be something that not > many people have an understanding of.Its not terrifically common, but it's not uncommon either. Try Wikipedia. Part of the reason you can't find information on the web is that you're asking a question whose answer is book length. I just got a copy of Dan Simon's "Optimal State Estimation", Wiley 2006. I recommend it if you're up to the math.> > This is my main question but I am also wondering how the A matrix is > derived. I can not see how the fusion of the gyro and accelerometer data > can be achieved with this matrix when fitting it to the form x=Ax+Bu. I > assume x would be [tilt angle, gyro bias] and u would be the gyro data.As pointed out elsewhere you have no absolute reference, so your system is not fully determined. Having some sort of absolute tilt or position measurement would get you a long way toward determinacy.> > Anybody got any ideas at all?? I am completly lost and confused. >All you can do with what you have is cage your system on the ground (i.e. find your drifts and initialize your tilt and position estimates), then use your control inputs to modify the various estimates when you're flying. I wouldn't even use a Kalman filter in this case -- I would just servo the platform rates to the turn inputs from the pilot, and the acceleration to the throttle input from the pilot, and see how flyable the thing is. -- Tim Wescott Wescott Design Services http://www.wescottdesign.com Posting from Google? See http://cfaj.freeshell.org/google/ "Applied Control Theory for Embedded Systems" came out in April. See details at http://www.wescottdesign.com/actfes/actfes.html

Reply by ●October 1, 20062006-10-01

quadrotor wrote:> This is my main question but I am also wondering how the A matrix is > derived.I too recommend Dan Simon's book. In his simple examples the systems keeps doing whatever it was doing as if the system was in space unaffected by gravity. Auto tuning systems for PID systems derive the A and B matrix values empirically. Real engineers can caluculate value for A and B.> I can not see how the fusion of the gyro and accelerometer data > can be achieved with this matrix when fitting it to the form x=Ax+Bu. I > assume x would be [tilt angle, gyro bias] and u would be the gyro data. > > Anybody got any ideas at all?? I am completly lost and confused.The estimated acceleration is subtracted from the accelerometer acceleration and mulitplied by the Kalman gain then added to Ax. The Bu term doesn't apply unless there is a forcing function. You just have to get a book on Kalman filters and work through the examples until you understand. See equations 2-5 http://www.innovatia.com/software/papers/kalman.htm Peter Nachtwey

Reply by ●October 1, 20062006-10-01

quadrotor wrote:> Why is the derivative used and where does that equation come from? I > havent been able to find anything on it. There doesnt seem to be a lot of > info around on the net and Kalman filtering seems to be something that not > many people have an understanding of. >that's an understatement, most Kalman filter info also tends to be circular, they all reference each other for specific details. Or you might read something like this "Kalman filtering is a huge field which cannot be explained in a brief paper.", you know either a big fat "Dead End" sign or 400 references are coming up shortly. I find things I understand I can explain to others in less then 2 pages, things I don't take a book.

Reply by ●October 1, 20062006-10-01

Tim Wescott wrote:> quadrotor wrote: >-- snip -->> >> This is my main question but I am also wondering how the A matrix is >> derived. I can not see how the fusion of the gyro and accelerometer data >> can be achieved with this matrix when fitting it to the form x=Ax+Bu. I >> assume x would be [tilt angle, gyro bias] and u would be the gyro data.-- snip -->I forgot to mention this before -- in a formal Kalman filter design the A matrix (the state evolution matrix in a continuous-time system, or the state transition matrix in a discrete-time system) is determined from building a careful model of the system, informed, as Peter has pointed out, with careful measurements if possible. This design step is where many Kalman filtering efforts break down -- the performance of your Kalman filter will suffer significantly if you get it wrong, so you have to either 'lie to the math' when you construct your problem by pretending to more noise than is really there, or you have to use alternate filtering strategies (like H-infinity state estimators). -- Tim Wescott Wescott Design Services http://www.wescottdesign.com Posting from Google? See http://cfaj.freeshell.org/google/ "Applied Control Theory for Embedded Systems" came out in April. See details at http://www.wescottdesign.com/actfes/actfes.html

Reply by ●October 1, 20062006-10-01

Thanks for the replies guys. I am looking to try and implement some sort of sensor fusion between the gyro and the accerlometer using a Kalman filter and I have see other examples of this being successful in other quadrotor systems. The tilt.c code I posted does exactly what I want along a single axis but I was just wondering about some of the functions used. Tim touched on why Pdot was used to update the P matrix and I have tried to do the maths to confirm that the result is the same as using the "normal" covariance update equation but have been unsuccessful. The A matrix as defined in tilt.c is also puzzling. http://coecsl.ece.uiuc.edu/ge423/spring04/group9/objectives_sensors.htm Above is another link that attemps to do something similiar. I can understand the method used here (the actualy definition of the system seems incorrect to me however as when its expanded it doesnt make any sense). Any mode ideas as to why Pdot is used to update and how they actually came up with the A matrix?? Thanks for your help guys. I was unable to find any other place on the net that was able to provide any help at all.

Reply by ●October 2, 20062006-10-02

Just another note now that I've had some time to digest whats been said. I only want the fusion to be accurate for near hover conditions. The angles determined using the accelerometers would be fairly accurate and without drift for near hover situations.

Reply by ●October 2, 20062006-10-02

Tim Wescott wrote:> quadrotor wrote: > >> Hi, >> >> I've been undertaking a project to create a controller for a quadrotor >> aircraft. Whilst the dynamic modelling and controller design has nearly >> been completed I am having trouble getting the requried feedback signals. >> >> I am trying to implement a Kalman filter to combine the signals of >> accelerometers and gyroscopes to produce an attitude estimate. I have >> read >> all the introductory literature and have also obtained some code to >> achieve >> the desired sensor fusion along a single axis. >> >> The code I have is here: 124.243.132.204\tilt.c >> >> The code uses the derivative of the covariance matrix to do the update: >> Pdot = A*P + P*A' + Q >> The P matrix is then updated using P=P+Pdot*dt >> >> The introductory papers however all use the update equation: >> P=A*P*A'+Q >> >> Why is the derivative used and where does that equation come from? I >> havent been able to find anything on it. > > I assume they use the derivative because they feel they'll get some > numerical advantage over doing the direct calculation. The expression > for Pdot doesn't look right to me, however -- I'm not saying it _isn't_ > right, but I'd have to dig into the math to verify it for myself. > >> There doesnt seem to be a lot of >> info around on the net and Kalman filtering seems to be something that >> not >> many people have an understanding of. > > Its not terrifically common, but it's not uncommon either. Try > Wikipedia. Part of the reason you can't find information on the web is > that you're asking a question whose answer is book length. I just got a > copy of Dan Simon's "Optimal State Estimation", Wiley 2006. I recommend > it if you're up to the math. >> >> This is my main question but I am also wondering how the A matrix is >> derived. I can not see how the fusion of the gyro and accelerometer data >> can be achieved with this matrix when fitting it to the form x=Ax+Bu. I >> assume x would be [tilt angle, gyro bias] and u would be the gyro data. > > As pointed out elsewhere you have no absolute reference, so your system > is not fully determined. Having some sort of absolute tilt or position > measurement would get you a long way toward determinacy. >> >> Anybody got any ideas at all?? I am completly lost and confused. >> > > All you can do with what you have is cage your system on the ground > (i.e. find your drifts and initialize your tilt and position estimates), > then use your control inputs to modify the various estimates when you're > flying. I wouldn't even use a Kalman filter in this case -- I would > just servo the platform rates to the turn inputs from the pilot, and the > acceleration to the throttle input from the pilot, and see how flyable > the thing is.And remember that for stability, the "stable platform" must have an undamped resonance with a period of 88 minutes. (88 minutes is the orbital time at zero altitude assuming no air, and also the period of a pendulum whose length is the earth's radius, assuming uniform gravity.) Jerry -- "The rights of the best of men are secured only as the rights of the vilest and most abhorrent are protected." - Chief Justice Charles Evans Hughes, 1927 ���������������������������������������������������������������������