RE: [Autopilot] 7 state Kalman filter on an MCU

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

Hello,
  OK, you convenced me.  Very creative.

Take Care,
Aaron Kahn 

-----Original Message-----
From: Trammell Hudson
To: Kahn, Aaron -AES
Cc: 'aut...@li... '
Sent: 5/21/2003 1:11 PM
Subject: Re: [Autopilot] 7 state Kalman filter on an MCU

On Wed, May 21, 2003 at 08:58:05AM -0500, Kahn, Aaron -AES wrote:
>   I have looked at the math portion of the code, in particular the
compass
> calculations, in the leveling portion.

Thanks for looking that over.  I find that to be the most difficult
part of the code right now...

> The calculation of the euler angle
> rototation matrix is correct, but the use of the dcm quaternion parts
is
> not.  If you want to use the quaternion elements, you still need to
compute
> what the psi value is first.

I'm not convinced of that.  Given the quaternion (including the psi
component), we can compute the pitch and roll Euler angles:

  Phi   = atan2( 2*(q2*q3 + q0*q1), 1-2*(q1*q1 + q2*q2))
  Theta = -asin( 2*(q1*q3 - q0*q2) )

Translating to the DCM form:

  Phi   = atan2( dcm12, dcm22 )
  Theta = -asin( dcm02 )

Additionally, from the Euler angle definition of the DCM, we know that:

  dcm12 = sin(Phi) * cos(Theta)


Using the computed rotation matrix for untilting:

[  cos(Theta)  sin(Phi)*sin(Theta)  cos(Phi)*sin(Theta) ]
[      0       cos(Phi)            -sin(Phi)            ]
[ -sin(Theta)  sin(Phi)*cos(Theta)  cos(Phi)*cos(Theta) ]

I did forget to transpose it; I'll fix that for the rest of this
discussion (as well as in the code).

[          cos(Theta)      0              -sin(Theta) ]
[ sin(Phi)*sin(Theta)   cos(phi)  sin(Phi)*cos(Theta) ]
[ cos(Phi)*sin(Theta)  -sin(Phi)  cos(Phi)*cos(Theta) ]

We should be able to substitute with our existing DCM without
any problems:

  sin(Phi) = dcm12 / sqrt( dcm12^2 + dcm22^2 )
  cos(Phi) = dcm22 / sqrt( dcm12^2 + dcm22^2 )
  sin(Theta) = -dcm02
  cos(Theta) = dcm12 / sin(Phi)
             = dcm12 / (dcm12 / sqrt( dcm12^2 + dcm22^2 ) )
             = sqrt( dcm12^2 + dcm22^2 )

  sin(Phi) = dcm12 / cos(Theta)
  cos(Phi) = dcm22 / cos(Theta)

The matrix is then very similar to what we had before:

[ cos(Theta)                    0            dcm02  ]
[ dcm12*-dcm02/cos(Theta)  dcm22/cos(Theta)  dcm12  ]
[          x                    x              x    ]

I've indicated that we do not care about the untilted Z component,
because it is not used for the eventual heading computation.

We can confirm that cos(Theta) = sqrt( dcm12^2 + dcm22^2 ):

  cos(Theta) = sqrt( dcm12^2 + dcm22^2 )
             = sqrt( (sin(Phi)cos(Theta))^2 + (cos(Phi)*cos(Theta))^2 )
             = sqrt( cos^2(Theta) * ( sin^2(Phi) + cos^2(Phi) )
             = cos(Theta) * sqrt( sin^2(Phi) + cos^2(Phi) )
	     = cos(Theta) * sqrt( 1 )
             = cos(Theta)

> [...]
> I am not sure if this saves anything more than using the euler angles.
You
> still need to do one sqrt and two trig functions.

I assume that our end results are the same, just that we took different
processes to get there.  My approach only requires one trig function
and two extra divisions, which I believe is less computationally
expensive.

Trammell
-- 
  -----|----- hu...@os...                   W 240-283-1700
*>=====[]L\   hu...@ro...                   M 505-463-1896
'     -'-`-   http://www.swcp.com/~hudson/                    KC5RNF

************************************ 
If this email is not intended for you, or you are not responsible for the
delivery of this message to the addressee, please note that this message may
contain ITT Privileged/Proprietary Information.  In such a case, you may not
copy or deliver this message to anyone.  You should destroy this message and
kindly notify the sender by reply email.  Information contained in this
message that does not relate to the business of ITT is neither endorsed by
nor attributable to ITT. 
************************************