Kalman Filter Help - Anybody understand Kalman filtering??

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.

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.
>
>
>
>
>

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

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

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

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.

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

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.

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.

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
���������������������������������������������������������������������