[Autopilot] RE: Question about ahrs.c (Revision 1.16)

[Bingwei S. <bs...@en...>]

Thanks for your reply. But for matrix C, the followling is my=
 results.

Definite the attitude quaternions vector as Q =3D (a b c d)'  
                        2 a b + 2 c d
  phi =3D atan(-------------------------) =3D atan(DCV1/DCV2)
                   a*a + d*d - b*b - c*c



 if diff(y, x)  differentiates y with respect to x then, 

                
                               2*b*DCV2 - 2*a*DCV1
diff(phi, a) =3D  --------------------------------------------
                         DCV1*DCV1 + DCV2*DCV2


                
                               2*a*DCV2 + 2*b*DCV1
diff(phi, b) =3D  --------------------------------------------
                         DCV1*DCV1 + DCV2*DCV2

                
                               2*d*DCV2 + 2*c*DCV1
diff(phi, c) =3D  --------------------------------------------
                         DCV1*DCV1 + DCV2*DCV2

                
                               2*c*DCV2 - 2*d*DCV1
diff(phi, d) =3D  --------------------------------------------
                         DCV1*DCV1 + DCV2*DCV2


From above results, I think the codes in attitude_update( void )

err =3D 2.0 / ( DCV[2] * DCV[2] + DCV[1] * DCV[1] );
C[0][0] =3D err * ( quat[1] * DCV[2] );
C[0][1] =3D err * ( quat[0] * DCV[2] + 2.0 * quat[1] * DCV[1] );
C[0][2] =3D err * ( quat[3] * DCV[2] + 2.0 * quat[2] * DCV[1] );
C[0][3] =3D err * ( quat[2] * DCV[2] );

Should be

C[0][0] =3D err * ( quat[1] * DCV[2] - quat[0] * DCV[1]);
C[0][1] =3D err * ( quat[0] * DCV[2] + quat[1] * DCV[1]);
C[0][2] =3D err * ( quat[3] * DCV[2] + quat[2] * DCV[1]);
C[0][3] =3D err * ( quat[2] * DCV[2] + quat[3] * DCV[1]);


>Message: 3
>From: "Kahn, Aaron -AES" <Aar...@it...>
>To: 'Tramm Hudson ' <hu...@sw...>, 
>=09"'aut...@li... '"
>=09 <aut...@li...>
>Subject: RE: [Autopilot] Question about ahrs.c (Revision 1.16)
>Date: Mon, 25 Nov 2002 12:50:14 -0500
>Reply-To: aut...@li...
>
>Hello,
>  First, on the question of the sign in the propogate state,=
 there is indeed
>an error.  Qdot[3] =3D (-p * quat[2] + q * quat[1]) * dt is the=
 correct
>equation.  Next, the C matrix in the update function represents=
 the partial
>derivative of the measurement to the state values.  If one takes=
 the partial
>derivative of the equations relating the quaternion to euler=
 angles, you can
>rearrange the terms to use the components of the DCM matrix. =
 One only needs
>to compute what you need.  In this case, you don't need all 9=
 elements, only
>3 are used.
>
>Hope that this answers some questions,
>Take Care,
>Aaron Kahn
>






=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D=3D
Bingwei SU
ENSICA, Dpt. Avionique & Syst=E8mes
1 place Emile Blouin
31056, Toulouse CEDEX 5
France
Tel: +33 5 61 61 85 94  Fax: +33 5 61 61 86 86
Email: bin...@en...