APM:Libraries
libraries
AP_NavEKF
Models
GimbalEstimatorExample
calcTmn.c
Go to the documentation of this file.
1
t2
= cos(gPsi);
2
t3
= sin(gTheta);
3
t4
= cos(gTheta);
4
t5
= sin(gPhi);
5
t6
= sin(gPsi);
6
t7
= q0*q0;
7
t8
= q1*q1;
8
t9
= q2*q2;
9
t10
= q3*q3;
10
t11
=
t7
+
t8
-
t9
-
t10
;
11
t12
= q0*q2*2.0;
12
t13
= q1*q3*2.0;
13
t14
=
t12
+
t13
;
14
t15
= cos(gPhi);
15
t16
= q0*q3*2.0;
16
t18
= q1*q2*2.0;
17
t17
=
t16
-
t18
;
18
t19
=
t2
*
t3
;
19
t20
=
t4
*
t5
*
t6
;
20
t21
=
t19
+
t20
;
21
t22
=
t2
*
t4
;
22
t34
=
t3
*
t5
*
t6
;
23
t23
=
t22
-
t34
;
24
t24
=
t4
*
t6
;
25
t25
=
t2
*
t3
*
t5
;
26
t26
=
t24
+
t25
;
27
t27
=
t16
+
t18
;
28
t28
=
t3
*
t6
;
29
t35
=
t2
*
t4
*
t5
;
30
t29
=
t28
-
t35
;
31
t30
= q0*q1*2.0;
32
t33
= q2*q3*2.0;
33
t31
=
t30
-
t33
;
34
t32
=
t7
-
t8
+
t9
-
t10
;
35
t36
=
t7
-
t8
-
t9
+
t10
;
36
t37
=
t12
-
t13
;
37
t38
=
t30
+
t33
;
38
A0
[0][0] =
t11
*
t23
+
t14
*
t21
+
t6
*
t15
*
t17
;
39
A0
[0][1] =
t11
*
t26
+
t14
*
t29
-
t2
*
t15
*
t17
;
40
A0
[0][2] = -
t5
*
t17
-
t3
*
t11
*
t15
+
t4
*
t14
*
t15
;
41
A0
[1][0] =
t23
*
t27
-
t21
*
t31
-
t6
*
t15
*
t32
;
42
A0
[1][1] =
t26
*
t27
-
t29
*
t31
+
t2
*
t15
*
t32
;
43
A0
[1][2] =
t5
*
t32
-
t3
*
t15
*
t27
-
t4
*
t15
*
t31
;
44
A0
[2][0] =
t21
*
t36
-
t23
*
t37
-
t6
*
t15
*
t38
;
45
A0
[2][1] = -
t26
*
t37
+
t29
*
t36
+
t2
*
t15
*
t38
;
46
A0
[2][2] =
t5
*
t38
+
t3
*
t15
*
t37
+
t4
*
t15
*
t36
;
t3
t3
Definition:
calcTmn.c:2
A0
A0[0][0]
Definition:
calcTmn.c:38
t19
t19
Definition:
calcTmn.c:18
t34
t34
Definition:
calcTmn.c:22
t27
t27
Definition:
calcTmn.c:27
t20
t20
Definition:
calcTmn.c:19
t37
t37
Definition:
calcTmn.c:36
t13
t13
Definition:
calcTmn.c:12
t8
t8
Definition:
calcTmn.c:7
t16
t16
Definition:
calcTmn.c:15
t21
t21
Definition:
calcTmn.c:20
t31
t31
Definition:
calcTmn.c:33
t12
t12
Definition:
calcTmn.c:11
t22
t22
Definition:
calcTmn.c:21
t38
t38
Definition:
calcTmn.c:37
t4
t4
Definition:
calcTmn.c:3
t7
t7
Definition:
calcTmn.c:6
t26
t26
Definition:
calcTmn.c:26
t32
t32
Definition:
calcTmn.c:34
t36
t36
Definition:
calcTmn.c:35
t30
t30
Definition:
calcTmn.c:31
t6
t6
Definition:
calcTmn.c:5
t17
t17
Definition:
calcTmn.c:17
t11
t11
Definition:
calcTmn.c:10
t29
t29
Definition:
calcTmn.c:30
t10
t10
Definition:
calcTmn.c:9
t23
t23
Definition:
calcTmn.c:23
t5
t5
Definition:
calcTmn.c:4
t24
t24
Definition:
calcTmn.c:24
t25
t25
Definition:
calcTmn.c:25
t28
t28
Definition:
calcTmn.c:28
t2
t2
Definition:
calcTmn.c:1
t14
t14
Definition:
calcTmn.c:13
t18
t18
Definition:
calcTmn.c:16
t9
t9
Definition:
calcTmn.c:8
t35
t35
Definition:
calcTmn.c:29
t15
t15
Definition:
calcTmn.c:14
t33
t33
Definition:
calcTmn.c:32
Generated on Sun Jun 17 2018 14:18:50 for APM:Libraries by
1.8.13