You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
47 lines
1.5 KiB
47 lines
1.5 KiB
/* |
|
Autocode for fusion of a yaw error measurement where the innovation is given by: |
|
|
|
innovation = atan2f(magMeasEarthFrameEast,magMeasEarthFrameNorth) - declinationAngle; |
|
|
|
magMeasEarthFrameEast and magMeasEarthFrameNorth are obtained by rotating the magnetometer measurements from body frame to eath frame. |
|
declinationAngle is the estimated declination as that location |
|
*/ |
|
|
|
// intermediate variables |
|
float t2 = q0*q0; |
|
float t3 = q1*q1; |
|
float t4 = q2*q2; |
|
float t5 = q3*q3; |
|
float t6 = q0*q2*2.0f; |
|
float t7 = q1*q3*2.0f; |
|
float t8 = t6+t7; |
|
float t9 = q0*q3*2.0f; |
|
float t13 = q1*q2*2.0f; |
|
float t10 = t9-t13; |
|
float t11 = t2+t3-t4-t5; |
|
float t12 = magX*t11; |
|
float t14 = magZ*t8; |
|
float t19 = magY*t10; |
|
float t15 = t12+t14-t19; |
|
float t16 = t2-t3+t4-t5; |
|
float t17 = q0*q1*2.0f; |
|
float t24 = q2*q3*2.0f; |
|
float t18 = t17-t24; |
|
float t20 = 1.0f/t15; |
|
float t21 = magY*t16; |
|
float t22 = t9+t13; |
|
float t23 = magX*t22; |
|
float t28 = magZ*t18; |
|
float t25 = t21+t23-t28; |
|
float t29 = t20*t25; |
|
float t26 = tan(t29); |
|
float t27 = 1.0f/(t15*t15); |
|
float t30 = t26*t26; |
|
float t31 = t30+1.0f; |
|
|
|
// Calculate the observation jacobian for the innovation derivative wrt the attitude error states only |
|
// Use the reduced order to optimise the calculation of the Kalman gain matrix and covariance update |
|
float H_MAG[3]; |
|
H_MAG[0] = -t31*(t20*(magZ*t16+magY*t18)+t25*t27*(magY*t8+magZ*t10)); |
|
H_MAG[1] = t31*(t20*(magX*t18+magZ*t22)+t25*t27*(magX*t8-magZ*t11)); |
|
H_MAG[2] = t31*(t20*(magX*t16-magY*t22)+t25*t27*(magX*t10+magY*t11));
|
|
|