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.5
f
);
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.5
f
-rotY*rotVarY*
t3
*
t11
*
t30
*0.5
f
;
47
P
[0][3] = rotX*rotVarX*
t3
*
t11
*(
t16
-rotX*rotZ*
t10
*
t12
*0.5f)*0.5
f
+rotY*rotVarY*
t3
*
t11
*(
t17
-rotY*rotZ*
t10
*
t12
*0.5
f
)*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.5
f
)-rotVarX*
t19
*
t28
-rotVarY*
t28
*
t30
;
51
P
[1][3] = rotVarY*(
t15
-
t23
)*(
t17
-rotY*rotZ*
t10
*
t12
*0.5
f
)-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.5
f
;
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.5
f
;
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
);
t39
float t39
Definition:
quatCovMat.c:36
t21
float t21
Definition:
quatCovMat.c:25
t24
float t24
Definition:
quatCovMat.c:27
t22
float t22
Definition:
quatCovMat.c:26
t30
float t30
Definition:
quatCovMat.c:31
t10
float t10
Definition:
quatCovMat.c:9
t8
float t8
Definition:
quatCovMat.c:6
t15
float t15
Definition:
quatCovMat.c:14
t44
float t44
Definition:
quatCovMat.c:43
t4
float t4
Definition:
quatCovMat.c:2
t29
float t29
Definition:
quatCovMat.c:29
t37
float t37
Definition:
quatCovMat.c:38
t40
float t40
Definition:
quatCovMat.c:33
t19
float t19
Definition:
quatCovMat.c:19
t32
float t32
Definition:
quatCovMat.c:32
t14
float t14
Definition:
quatCovMat.c:13
t28
float t28
Definition:
quatCovMat.c:21
t9
float t9
Definition:
quatCovMat.c:8
t42
float t42
Definition:
quatCovMat.c:41
t27
float t27
Definition:
quatCovMat.c:18
t11
float t11
Definition:
quatCovMat.c:10
t17
float t17
Definition:
quatCovMat.c:16
t31
float t31
Definition:
quatCovMat.c:24
t33
float t33
Definition:
quatCovMat.c:34
t5
float t5
Definition:
quatCovMat.c:3
t43
float t43
Definition:
quatCovMat.c:42
t16
float t16
Definition:
quatCovMat.c:15
t20
float t20
Definition:
quatCovMat.c:22
f
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition:
integration.cpp:8
t2
float t2
Definition:
quatCovMat.c:1
t7
float t7
Definition:
quatCovMat.c:5
t35
float t35
Definition:
quatCovMat.c:37
t41
float t41
Definition:
quatCovMat.c:40
t25
float t25
Definition:
quatCovMat.c:23
t13
float t13
Definition:
quatCovMat.c:12
t6
float t6
Definition:
quatCovMat.c:4
P
P[0][0]
Definition:
quatCovMat.c:44
t23
float t23
Definition:
quatCovMat.c:20
t26
float t26
Definition:
quatCovMat.c:28
t34
float t34
Definition:
quatCovMat.c:30
t38
float t38
Definition:
quatCovMat.c:39
t12
float t12
Definition:
quatCovMat.c:11
t36
float t36
Definition:
quatCovMat.c:35
t3
float t3
Definition:
quatCovMat.c:7
t18
float t18
Definition:
quatCovMat.c:17
src
lib
ecl
EKF
matlab
scripts
Inertial Nav EKF
quatCovMat.c
Generated by
1.8.13