PX4 Firmware
PX4 Autopilot Software http://px4.io
quatCovMat.c
Go to the documentation of this file.
1 float t2 = rotX*rotX;
2 float t4 = rotY*rotY;
3 float t5 = rotZ*rotZ;
4 float t6 = t2+t4+t5;
5 float t7 = sqrtf(t6);
6 float t8 = t7*0.5f;
7 float t3 = sinf(t8);
8 float t9 = t3*t3;
9 float t10 = 1.0f/t6;
10 float t11 = 1.0f/sqrtf(t6);
11 float t12 = cosf(t8);
12 float t13 = 1.0f/powf(t6,1.5f);
13 float t14 = t3*t11;
14 float t15 = rotX*rotY*t3*t13;
15 float t16 = rotX*rotZ*t3*t13;
16 float t17 = rotY*rotZ*t3*t13;
17 float t18 = t2*t10*t12*0.5f;
18 float t27 = t2*t3*t13;
19 float t19 = t14+t18-t27;
20 float t23 = rotX*rotY*t10*t12*0.5f;
21 float t28 = t15-t23;
22 float t20 = rotY*rotVarY*t3*t11*t28*0.5f;
23 float t25 = rotX*rotZ*t10*t12*0.5f;
24 float t31 = t16-t25;
25 float t21 = rotZ*rotVarZ*t3*t11*t31*0.5f;
26 float t22 = t20+t21-rotX*rotVarX*t3*t11*t19*0.5f;
27 float t24 = t15-t23;
28 float t26 = t16-t25;
29 float t29 = t4*t10*t12*0.5f;
30 float t34 = t3*t4*t13;
31 float t30 = t14+t29-t34;
32 float t32 = t5*t10*t12*0.5f;
33 float t40 = t3*t5*t13;
34 float t33 = t14+t32-t40;
35 float t36 = rotY*rotZ*t10*t12*0.5f;
36 float t39 = t17-t36;
37 float t35 = rotZ*rotVarZ*t3*t11*t39*0.5f;
38 float t37 = t15-t23;
39 float t38 = t17-t36;
40 float t41 = rotVarX*(t15-t23)*(t16-t25);
41 float t42 = t41-rotVarY*t30*t39-rotVarZ*t33*t39;
42 float t43 = t16-t25;
43 float t44 = t17-t36;
44 P[0][0] = rotVarX*t2*t9*t10*0.25f+rotVarY*t4*t9*t10*0.25f+rotVarZ*t5*t9*t10*0.25f;
45 P[0][1] = t22;
46 P[0][2] = t35+rotX*rotVarX*t3*t11*(t15-rotX*rotY*t10*t12*0.5f)*0.5f-rotY*rotVarY*t3*t11*t30*0.5f;
47 P[0][3] = rotX*rotVarX*t3*t11*(t16-rotX*rotZ*t10*t12*0.5f)*0.5f+rotY*rotVarY*t3*t11*(t17-rotY*rotZ*t10*t12*0.5f)*0.5f-rotZ*rotVarZ*t3*t11*t33*0.5f;
48 P[1][0] = t22;
49 P[1][1] = rotVarX*(t19*t19)+rotVarY*(t24*t24)+rotVarZ*(t26*t26);
50 P[1][2] = rotVarZ*(t16-t25)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t28-rotVarY*t28*t30;
51 P[1][3] = rotVarY*(t15-t23)*(t17-rotY*rotZ*t10*t12*0.5f)-rotVarX*t19*t31-rotVarZ*t31*t33;
52 P[2][0] = t35-rotY*rotVarY*t3*t11*t30*0.5f+rotX*rotVarX*t3*t11*(t15-t23)*0.5f;
53 P[2][1] = rotVarZ*(t16-t25)*(t17-t36)-rotVarX*t19*t28-rotVarY*t28*t30;
54 P[2][2] = rotVarY*(t30*t30)+rotVarX*(t37*t37)+rotVarZ*(t38*t38);
55 P[2][3] = t42;
56 P[3][0] = rotZ*rotVarZ*t3*t11*t33*(-1.0f/2.0f)+rotX*rotVarX*t3*t11*(t16-t25)*0.5f+rotY*rotVarY*t3*t11*(t17-t36)*0.5f;
57 P[3][1] = rotVarY*(t15-t23)*(t17-t36)-rotVarX*t19*t31-rotVarZ*t31*t33;
58 P[3][2] = t42;
59 P[3][3] = rotVarZ*(t33*t33)+rotVarX*(t43*t43)+rotVarY*(t44*t44);
float t39
Definition: quatCovMat.c:36
float t21
Definition: quatCovMat.c:25
float t24
Definition: quatCovMat.c:27
float t22
Definition: quatCovMat.c:26
float t30
Definition: quatCovMat.c:31
float t10
Definition: quatCovMat.c:9
float t8
Definition: quatCovMat.c:6
float t15
Definition: quatCovMat.c:14
float t44
Definition: quatCovMat.c:43
float t4
Definition: quatCovMat.c:2
float t29
Definition: quatCovMat.c:29
float t37
Definition: quatCovMat.c:38
float t40
Definition: quatCovMat.c:33
float t19
Definition: quatCovMat.c:19
float t32
Definition: quatCovMat.c:32
float t14
Definition: quatCovMat.c:13
float t28
Definition: quatCovMat.c:21
float t9
Definition: quatCovMat.c:8
float t42
Definition: quatCovMat.c:41
float t27
Definition: quatCovMat.c:18
float t11
Definition: quatCovMat.c:10
float t17
Definition: quatCovMat.c:16
float t31
Definition: quatCovMat.c:24
float t33
Definition: quatCovMat.c:34
float t5
Definition: quatCovMat.c:3
float t43
Definition: quatCovMat.c:42
float t16
Definition: quatCovMat.c:15
float t20
Definition: quatCovMat.c:22
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
float t2
Definition: quatCovMat.c:1
float t7
Definition: quatCovMat.c:5
float t35
Definition: quatCovMat.c:37
float t41
Definition: quatCovMat.c:40
float t25
Definition: quatCovMat.c:23
float t13
Definition: quatCovMat.c:12
float t6
Definition: quatCovMat.c:4
P[0][0]
Definition: quatCovMat.c:44
float t23
Definition: quatCovMat.c:20
float t26
Definition: quatCovMat.c:28
float t34
Definition: quatCovMat.c:30
float t38
Definition: quatCovMat.c:39
float t12
Definition: quatCovMat.c:11
float t36
Definition: quatCovMat.c:35
float t3
Definition: quatCovMat.c:7
float t18
Definition: quatCovMat.c:17