PX4 Firmware
PX4 Autopilot Software http://px4.io
calcMAGD.c
Go to the documentation of this file.
1
t2
= magE*magE;
2
t3
= magN*magN;
3
t4
=
t2
+
t3
;
4
t5
=
P
[16][16]*
t2
;
5
t6
=
P
[17][17]*
t3
;
6
t7
=
t2
*
t2
;
7
t8
= R_DECL*
t7
;
8
t9
=
t3
*
t3
;
9
t10
= R_DECL*
t9
;
10
t11
= R_DECL*
t2
*
t3
*2.0;
11
t14
=
P
[16][17]*magE*magN;
12
t15
=
P
[17][16]*magE*magN;
13
t12
=
t5
+
t6
+
t8
+
t10
+
t11
-
t14
-
t15
;
14
t13
= 1.0/
t12
;
15
t16
= conjugate(magE);
16
t17
= conjugate(magN);
17
t18
=
t16
*
t16
;
18
t19
=
t17
*
t17
;
19
t20
=
t18
+
t19
;
20
t21
= 1.0/
t20
;
21
A0
[0][0] = -
t4
*
t13
*(
P
[0][16]*magE-
P
[0][17]*magN);
22
A0
[1][0] = -
t4
*
t13
*(
P
[1][16]*magE-
P
[1][17]*magN);
23
A0
[2][0] = -
t4
*
t13
*(
P
[2][16]*magE-
P
[2][17]*magN);
24
A0
[3][0] = -
t4
*
t13
*(
P
[3][16]*magE-
P
[3][17]*magN);
25
A0
[4][0] = -
t4
*
t13
*(
P
[4][16]*magE-
P
[4][17]*magN);
26
A0
[5][0] = -
t4
*
t13
*(
P
[5][16]*magE-
P
[5][17]*magN);
27
A0
[6][0] = -
t4
*
t13
*(
P
[6][16]*magE-
P
[6][17]*magN);
28
A0
[7][0] = -
t4
*
t13
*(
P
[7][16]*magE-
P
[7][17]*magN);
29
A0
[8][0] = -
t4
*
t13
*(
P
[8][16]*magE-
P
[8][17]*magN);
30
A0
[9][0] = -
t4
*
t13
*(
P
[9][16]*magE-
P
[9][17]*magN);
31
A0
[10][0] = -
t4
*
t13
*(
P
[10][16]*magE-
P
[10][17]*magN);
32
A0
[11][0] = -
t4
*
t13
*(
P
[11][16]*magE-
P
[11][17]*magN);
33
A0
[12][0] = -
t4
*
t13
*(
P
[12][16]*magE-
P
[12][17]*magN);
34
A0
[13][0] = -
t4
*
t13
*(
P
[13][16]*magE-
P
[13][17]*magN);
35
A0
[14][0] = -
t4
*
t13
*(
P
[14][16]*magE-
P
[14][17]*magN);
36
A0
[15][0] = -
t4
*
t13
*(
P
[15][16]*magE-
P
[15][17]*magN);
37
A0
[16][0] = -
t4
*
t13
*(
P
[16][16]*magE-
P
[16][17]*magN);
38
A0
[16][1] = -
t16
*
t21
;
39
A0
[17][0] = -
t4
*
t13
*(
P
[17][16]*magE-
P
[17][17]*magN);
40
A0
[17][1] =
t17
*
t21
;
41
A0
[18][0] = -
t4
*
t13
*(
P
[18][16]*magE-
P
[18][17]*magN);
42
A0
[19][0] = -
t4
*
t13
*(
P
[19][16]*magE-
P
[19][17]*magN);
43
A0
[20][0] = -
t4
*
t13
*(
P
[20][16]*magE-
P
[20][17]*magN);
44
A0
[21][0] = -
t4
*
t13
*(
P
[21][16]*magE-
P
[21][17]*magN);
45
A0
[22][0] = -
t4
*
t13
*(
P
[22][16]*magE-
P
[22][17]*magN);
46
A0
[23][0] = -
t4
*
t13
*(
P
[23][16]*magE-
P
[23][17]*magN);
t10
t10
Definition:
calcMAGD.c:9
t19
t19
Definition:
calcMAGD.c:18
t2
t2
Definition:
calcMAGD.c:1
t13
t13
Definition:
calcMAGD.c:14
t17
t17
Definition:
calcMAGD.c:16
t21
t21
Definition:
calcMAGD.c:20
A0
A0[0][0]
Definition:
calcMAGD.c:21
t11
t11
Definition:
calcMAGD.c:10
t8
t8
Definition:
calcMAGD.c:7
t9
t9
Definition:
calcMAGD.c:8
t12
t12
Definition:
calcMAGD.c:13
t4
t4
Definition:
calcMAGD.c:3
t20
t20
Definition:
calcMAGD.c:19
t14
t14
Definition:
calcMAGD.c:11
t7
t7
Definition:
calcMAGD.c:6
t18
t18
Definition:
calcMAGD.c:17
t3
t3
Definition:
calcMAGD.c:2
t16
t16
Definition:
calcMAGD.c:15
t15
t15
Definition:
calcMAGD.c:12
P
P[0][0]
Definition:
quatCovMat.c:44
t6
t6
Definition:
calcMAGD.c:5
t5
t5
Definition:
calcMAGD.c:4
src
lib
ecl
EKF
matlab
scripts
Inertial Nav EKF
calcMAGD.c
Generated by
1.8.13