RE: [Autopilot] RE: Kalman posts on autopilot,sourceforge

[Kahn, A. - A. <Aar...@it...>]

Hello,
  By Golly I think you got it!  Good work.  Remember though, that=
 this is just a first order linearization taken at each time step (as dic=
tated by the Extended Kalman Filter implementation).  The only reason tha=
t it is done this way is to make the math easier.  There are two ways of =
tackling this section of the filter.
1) Compute the measurement in quate=
rnions and have C =3D I(4x4).
2) Compute the measurement in euler angles=
, convert the state to euler angles and have C =3D [dphi/dQ dtheta/dQ dps=
i/dQ] (4x4).

If you work through the math, the second approach, while =
at the outset sounds harder, is actuatally much simpler.  The first appro=
ach will in the end involve some tricky math dealing with the subraction =
of 2 quaternions, both of which might not be fully defined (ie: all three=
 angles).  I have tried to 1, but failed.

Take Care,
Aaron Kahn 

-=
----Original Message-----
From: aut...@li...urceforge.=
net
To: 'S=F8ren Madsen'; aut...@li...
Cc: Iai=
n Peddle; Kahn, Aaron - AES; MILNE@SUN.AC.ZA
Sent: 10/3/2003 11:12 AM
S=
ubject: [Autopilot] RE: Kalman posts on autopilot,sourceforge

Hi.

I=
 have taken some time to respond to this one... but I think I have
final=
ly seen the light!! (Once again not rocket science once you know
the ans=
wer ;-)

I believe the DCM, and the inverse of it, are just matrices to=
 convert
between body frame and navigation (inertial) frame. But the DCM=
 is not
the C (or H - depending on your notation) matrix. Don't be foole=
d by the
apparent similarities.

Why is the C matrix =3D [ dphi/dQ ; d=
theta/dQ; dpsi/dQ ] ? Let's limit
this discussion to the 4 state version=
 (since adding the 3 biases are
just trivial complications once we have =
this one cracked). This one took
me a while... but just write it out, an=
d it makes sense. The outputs of
the "plant" are the Euler angles (easy =
to see if you remember that you
are subtracting the angles calculated us=
ing the accelerometers from the
estimated angles). Thus... (Q is the qua=
ternion and state vector)

Euler angles =3D Y =3D C * Q

d(Euler)/dQ =
=3D C * I =3D C!

And that is what you were in need of: C (to be able t=
o subtract apples
from apples).

If you look at the structure of the K=
alman filter (well, an estimator),
then you have to subtract estimated f=
rom measured outputs... So... You
are measuring the Euler attitude angle=
s, thus you have to transform your
Quaternion states back to Euler angle=
s... You will see that at the
beginning of imuUpdate.m.

Aaron, am I t=
alking nonsense? 

Good luck.
Nicol.

PS: In the last year we have h=
ad 4 exchange students from the
Netherlands, one from Germany and 2 from=
 Austria.
http://esl.ee.sun.ac.za/

-----Original Message-----
From: =
S=F8ren Madsen [mailto:s_m...@st...] 
Sent: 02 October 2003 17:0=
0
To: car...@su...
Subject: Re: Kalman posts on autopilot,sourcef=
orge

Hi

Your help really cracked the nut for me! The only thing I h=
ave trouble
following in your/Aron's code is the construction of the 'C'=
 matrix. I
gather that whats going on is forming the matrix:

    d [ =
phi  theta  psi ]
   --------------------        , where Q is the quater=
nion part of the
state
vector
          d Q

But the way its done us=
ing the DCM matrix I don't fully understand, do
you?
I successfully Goo=
gle'd for the R=F6nnb=E4ck thesis myself, but thanks for
the
offer!

=
Best regards
S=F8ren

PS: I don't mind you asking at all! I'm from Den=
mark, which is a small
country
(total population ~ 6 mill. people) nort=
h of Germany and right next to
Sweden.

----- Original Message -----
=
From: "Nicol Carstens" <car...@su...>
To: "'S=F8ren Madsen'" <s_ma=
ds...@st...>
Sent: Wednesday, October 01, 2003 8:19 PM
Subject: RE=
: Kalman posts on autopilot,sourceforge


Hi...

Glad I could help :=
-)

There might be one or two (hopefully not more) errors in there. I h=
ave
found the thesis using Google.com. If your email can handle it, I ca=
n
send it via email. I have it with me...

"defacto" ... well, are the=
re really any "standards" that you can call
defacto!

>> I had'nt noti=
ced the Kalman page at autopilot.sourceforge!

Yes. It is not easily ac=
cessible -like it use to be before the new
website. It is, as a matter o=
f fact, part of the old site I believe. Why
I am not quite sure, since i=
t contains some good information.

Regards, and good luck,
Nicol.

P=
S: mind if I ask from where you are? We have some international
students=
 visiting every now-and-then.


-----Original Message-----
From: S=F8=
ren Madsen [mailto:s_m...@st...]
Sent: 01 October 2003 19:04
To=
: car...@su...
Subject: Re: Kalman posts on autopilot,sourceforge
=

Thank you, thank you, thank you!
Didn't know the pqr defacto standard.=

I had'nt noticed the Kalman page at autopilot.sourceforge! I'll
try to=
 get my hands on  Sven Ronnback's thesis too.
By the way, your answers m=
ade perfect sense! I'm studying
electrical engineering and have had a co=
urse on Kalman
filtering my previous semester, so that's my background!
=

Thank you for your trouble!
S=F8ren Madsen

----- Original Message -=
----
From: "Nicol Carstens" <car...@su...>
To: "'Madsen S=B0ren C=
hristian'" <200...@ei...>;
<aut...@li...=
>
Sent: Wednesday, October 01, 2003 1:40 PM
Subject: RE: Kalman posts o=
n autopilot,sourceforge


Hello.

>> The pqr are the roll rates, rig=
ht?

Yes...

>> but how are they
>> organized, does 'q' correspond t=
o yaw?

p roll rate
q pitch rate
r yaw rate

Fairly standard, and r=
espected by most.

>> I'm a bit puzzled by the A matrix (I think its th=
e
>> conversions back and forth between from euler
>> angles and quater=
nions that causes it...), how is it
>> defined? Comparing to the Extende=
d Kalman filter
>> in "Applied Optimal Estimation", does the A matrix
>=
> correspond to F( Xhat(t),t ) =3D d f( X(t), t)/ d X(t) ?

Ok... now t=
his is where is gets a bit more difficult to answer questions
via email,=
 especially since I don't know much about your background -
and I still =
have a lot to learn myself. How much do you know about
control systems t=
erminology and symbols?  One of the first things you
will learn when wor=
king with state space equations is...

d(X)/dt =3D AX + BU
Y =3D CX + =
DU

... in the continues domain. (NB: this is not the same C as on the =
above
mentioned website - not the DCM) If you go to a discrete system, i=
t will
become

X(k+1) =3D  FX(k) + GU(k)
Y(k) =3D HX(k) + JU(k)

..=
. but G and H are typically also replaced with theta or gamma ... so
the=
re are some variations when it comes to symbols.

X is referred to as t=
he states of the system / plant. The A matrix
describes the dynamics of =
the plant. Where these linear differential
equations come from depends o=
n the type of plant (it could be the
voltage of a capacitor, the movemen=
t of a car, the trajectory of a
missile or anything else you might want =
to control).

This plant is a bit more abstract than might be presented=
 in most basic
textbook examples. There is a set of equations that descr=
ibes the
relationship between quaternions and euler angles and rates (se=
e for
example the thesis of Sven Ronnback "Development of an INS/GPS
na=
vigation loop for an UAV" for more detail on the quaternion equations
..=
. LTU-EX-00081-SE.pdf). That has been described on the autopilot
webpage=
 on the KF (http://autopilot.sourceforge.net/kalman.html).

There are 3=
 "types" of states: true, measured and estimated. The true
states are wh=
at we would love to know to be able to apply the perfect
control signal.=
 The measured values are the states that we can measure
using sensors...=
 but they contain noise and not all states can always be
measured using =
sensor. Therefore, to get better estimates of the true
states... we use =
an estimator. The kalman filter is "a tool that tells
you how much you h=
ave to adjust your estimation" - many people will
shoot me for this! But=
 the KF gains are used to get the best possible
estimate using the senso=
rs, and given the known amount of noise you are
expecting on the sensors=
.

What I wanted to say is that estimated states are usually indicated
=
using a hat.

>> Hope my questions make sense!

Hope my answers make =
some sense :-) If most of what you saw here was
news to you, you might b=
enefit a great deal from looking at basic
control systems book.

To th=
e rest... please object if I have made gross errors in my
description. N=
o offence will be taken :)

Regards,
Nicol.









-------=
------------------------------------------------
This SF.net email is sp=
onsored by: SF.net Giveback Program.
Does SourceForge.net help you be mo=
re productive?  Does it
help you create better code?   SHARE THE LOVE, a=
nd help us help
YOU!  Click Here: http://sourceforge.net/donate/
______=
_________________________________________
http://autopilot.sourceforge.n=
et/     Development mailing list
https://lists.sourceforge.net/lists/lis=
tinfo/autopilot-devel

************************************
This email=
 and any files transmitted with it are proprietary and intended solely fo=
r the use of the individual or entity to whom they are addressed. If you =
have received this email in error please notify the sender. Please note t=
hat any views or opinions presented in this email are solely those of the=
 author and do not necessarily represent those of ITT Industries, Inc. Th=
e recipient should check this email and any attachments for the presence =
of viruses. ITT Industries accepts no liability for any damage caused by =
any virus transmitted by this email.
***********************************=
*