PX4 Firmware
PX4 Autopilot Software http://px4.io
YawCovariance.c
Go to the documentation of this file.
1 /*
2  C code fragment for function that enables the yaw uncertainty to be increased following a yaw reset.
3  The variables _state.quat_nominal(0) -> _state.quat_nominal(3) are the attitude quaternions
4  The variable daYawVar is the variance of the yaw angle uncertainty in rad**2
5  See DeriveYawResetEquations.m for the derivation
6  The gnerate autocode has been cleaned up with removal of 0 coefficient terms and mirroring of lower
7  diagonal terms missing from the derivation script raw autocode output of C_code4.txt
8 */
9 
10 // Intermediate variables
11 float SG[3];
12 SG[0] = sq(_state.quat_nominal(0)) - sq(_state.quat_nominal(1)) - sq(_state.quat_nominal(2)) + sq(_state.quat_nominal(3));
13 SG[1] = 2*_state.quat_nominal(0)*_state.quat_nominal(2) - 2*_state.quat_nominal(1)*_state.quat_nominal(3);
14 SG[2] = 2*_state.quat_nominal(0)*_state.quat_nominal(1) + 2*_state.quat_nominal(2)*_state.quat_nominal(3);
15 
16 float SQ[4];
17 SQ[0] = 0.5f * ((_state.quat_nominal(1)*SG[0]) - (_state.quat_nominal(0)*SG[2]) + (_state.quat_nominal(3)*SG[1]));
18 SQ[1] = 0.5f * ((_state.quat_nominal(0)*SG[1]) - (_state.quat_nominal(2)*SG[0]) + (_state.quat_nominal(3)*SG[2]));
19 SQ[2] = 0.5f * ((_state.quat_nominal(3)*SG[0]) - (_state.quat_nominal(1)*SG[1]) + (_state.quat_nominal(2)*SG[2]));
20 SQ[3] = 0.5f * ((_state.quat_nominal(0)*SG[0]) + (_state.quat_nominal(1)*SG[2]) + (_state.quat_nominal(2)*SG[1]));
21 
22 // Variance of yaw angle uncertainty (rad**2)
23 const float daYawVar = TBD;
24 
25 // Add covariances for additonal yaw uncertainty to existing covariances.
26 // This assumes that the additional yaw error is uncorrrelated
27 P[0][0] += yaw_variance*sq(SQ[2]);
28 P[0][1] += yaw_variance*SQ[1]*SQ[2];
29 P[1][1] += yaw_variance*sq(SQ[1]);
30 P[0][2] += yaw_variance*SQ[0]*SQ[2];
31 P[1][2] += yaw_variance*SQ[0]*SQ[1];
32 P[2][2] += yaw_variance*sq(SQ[0]);
33 P[0][3] -= yaw_variance*SQ[2]*SQ[3];
34 P[1][3] -= yaw_variance*SQ[1]*SQ[3];
35 P[2][3] -= yaw_variance*SQ[0]*SQ[3];
36 P[3][3] += yaw_variance*sq(SQ[3]);
37 P[1][0] += yaw_variance*SQ[1]*SQ[2];
38 P[2][0] += yaw_variance*SQ[0]*SQ[2];
39 P[2][1] += yaw_variance*SQ[0]*SQ[1];
40 P[3][0] -= yaw_variance*SQ[2]*SQ[3];
41 P[3][1] -= yaw_variance*SQ[1]*SQ[3];
42 P[3][2] -= yaw_variance*SQ[0]*SQ[3];
float SQ[4]
Definition: YawCovariance.c:16
float SG[3]
Definition: YawCovariance.c:11
const float daYawVar
Definition: YawCovariance.c:23
P[0][0]
Definition: YawCovariance.c:27