PX4 Firmware
PX4 Autopilot Software http://px4.io
LOSX.c
Go to the documentation of this file.
1
float
t2
= 1.0f/range;
2
float
t3
= Tbs.b.y*q0*2.0f;
3
float
t4
= Tbs.b.x*q3*2.0f;
4
float
t18
= Tbs.b.z*q1*2.0f;
5
float
t5
=
t3
+
t4
-
t18
;
6
float
t6
= Tbs.b.y*q1*2.0f;
7
float
t7
= Tbs.b.z*q0*2.0f;
8
float
t16
= Tbs.b.x*q2*2.0f;
9
float
t8
=
t6
+
t7
-
t16
;
10
float
t9
= Tbs.b.x*q0*2.0f;
11
float
t10
= Tbs.b.z*q2*2.0f;
12
float
t17
= Tbs.b.y*q3*2.0f;
13
float
t11
=
t9
+
t10
-
t17
;
14
float
t12
= Tbs.b.x*q1*2.0f;
15
float
t13
= Tbs.b.y*q2*2.0f;
16
float
t14
= Tbs.b.z*q3*2.0f;
17
float
t15
=
t12
+
t13
+
t14
;
18
float
t19
= q0*q0;
19
float
t20
= q1*q1;
20
float
t21
= q2*q2;
21
float
t22
= q3*q3;
22
float
t23
= q0*q3*2.0f;
23
float
t24
= q0*q2*2.0f;
24
float
t25
= q1*q3*2.0f;
25
float
t26
= q0*q1*2.0f;
26
float
t27
=
t19
+
t20
-
t21
-
t22
;
27
float
t28
= Tbs.b.x*
t27
;
28
float
t29
= q1*q2*2.0f;
29
float
t30
=
t24
+
t25
;
30
float
t31
= Tbs.b.z*
t30
;
31
float
t32
=
t19
-
t20
+
t21
-
t22
;
32
float
t33
= Tbs.b.y*
t32
;
33
float
t34
=
t23
+
t29
;
34
float
t35
= Tbs.b.x*
t34
;
35
float
t36
= q2*q3*2.0f;
36
float
t37
=
t19
-
t20
-
t21
+
t22
;
37
float
t38
= Tbs.b.z*
t37
;
38
float
t39
=
t24
-
t25
;
39
float
t40
=
t26
+
t36
;
40
float
t41
= Tbs.b.y*
t40
;
41
float
t60
= Tbs.b.x*
t39
;
42
float
t42
=
t38
+
t41
-
t60
;
43
float
t43
=
t8
*vd;
44
float
t44
=
t5
*ve;
45
float
t45
=
t11
*vn;
46
float
t46
=
t43
+
t44
+
t45
;
47
float
t47
=
t5
*vd;
48
float
t48
=
t15
*vn;
49
float
t62
=
t8
*ve;
50
float
t49
=
t47
+
t48
-
t62
;
51
float
t50
=
t15
*ve;
52
float
t51
=
t8
*vn;
53
float
t63
=
t11
*vd;
54
float
t52
=
t50
+
t51
-
t63
;
55
float
t53
=
t15
*vd;
56
float
t54
=
t11
*ve;
57
float
t64
=
t5
*vn;
58
float
t55
=
t53
+
t54
-
t64
;
59
float
t56
=
t23
-
t29
;
60
float
t65
= Tbs.b.y*
t56
;
61
float
t57
=
t28
+
t31
-
t65
;
62
float
t58
=
t26
-
t36
;
63
float
t66
= Tbs.b.z*
t58
;
64
float
t59
=
t33
+
t35
-
t66
;
65
float
t61
=
P
[0][0]*
t2
*
t46
;
66
float
t67
=
P
[1][1]*
t2
*
t49
;
67
float
t68
=
P
[4][0]*
t2
*
t57
;
68
float
t69
=
P
[5][0]*
t2
*
t59
;
69
float
t70
=
P
[6][0]*
t2
*
t42
;
70
float
t71
=
P
[1][0]*
t2
*
t49
;
71
float
t72
=
P
[2][0]*
t2
*
t52
;
72
float
t73
=
P
[3][0]*
t2
*
t55
;
73
float
t74
=
t61
+
t68
+
t69
+
t70
+
t71
+
t72
+
t73
;
74
float
t75
=
t2
*
t46
*
t74
;
75
float
t76
=
P
[4][1]*
t2
*
t57
;
76
float
t77
=
P
[5][1]*
t2
*
t59
;
77
float
t78
=
P
[6][1]*
t2
*
t42
;
78
float
t79
=
P
[0][1]*
t2
*
t46
;
79
float
t80
=
P
[2][1]*
t2
*
t52
;
80
float
t81
=
P
[3][1]*
t2
*
t55
;
81
float
t82
=
t67
+
t76
+
t77
+
t78
+
t79
+
t80
+
t81
;
82
float
t83
=
t2
*
t49
*
t82
;
83
float
t84
=
P
[4][2]*
t2
*
t57
;
84
float
t85
=
P
[5][2]*
t2
*
t59
;
85
float
t86
=
P
[6][2]*
t2
*
t42
;
86
float
t87
=
P
[0][2]*
t2
*
t46
;
87
float
t88
=
P
[1][2]*
t2
*
t49
;
88
float
t89
=
P
[2][2]*
t2
*
t52
;
89
float
t90
=
P
[3][2]*
t2
*
t55
;
90
float
t91
=
t84
+
t85
+
t86
+
t87
+
t88
+
t89
+
t90
;
91
float
t92
=
t2
*
t52
*
t91
;
92
float
t93
=
P
[4][3]*
t2
*
t57
;
93
float
t94
=
P
[5][3]*
t2
*
t59
;
94
float
t95
=
P
[6][3]*
t2
*
t42
;
95
float
t96
=
P
[0][3]*
t2
*
t46
;
96
float
t97
=
P
[1][3]*
t2
*
t49
;
97
float
t98
=
P
[2][3]*
t2
*
t52
;
98
float
t99
=
P
[3][3]*
t2
*
t55
;
99
float
t100
=
t93
+
t94
+
t95
+
t96
+
t97
+
t98
+
t99
;
100
float
t101
=
t2
*
t55
*
t100
;
101
float
t102
=
P
[4][4]*
t2
*
t57
;
102
float
t103
=
P
[5][4]*
t2
*
t59
;
103
float
t104
=
P
[6][4]*
t2
*
t42
;
104
float
t105
=
P
[0][4]*
t2
*
t46
;
105
float
t106
=
P
[1][4]*
t2
*
t49
;
106
float
t107
=
P
[2][4]*
t2
*
t52
;
107
float
t108
=
P
[3][4]*
t2
*
t55
;
108
float
t109
=
t102
+
t103
+
t104
+
t105
+
t106
+
t107
+
t108
;
109
float
t110
=
t2
*
t57
*
t109
;
110
float
t111
=
P
[4][5]*
t2
*
t57
;
111
float
t112
=
P
[5][5]*
t2
*
t59
;
112
float
t113
=
P
[6][5]*
t2
*
t42
;
113
float
t114
=
P
[0][5]*
t2
*
t46
;
114
float
t115
=
P
[1][5]*
t2
*
t49
;
115
float
t116
=
P
[2][5]*
t2
*
t52
;
116
float
t117
=
P
[3][5]*
t2
*
t55
;
117
float
t118
=
t111
+
t112
+
t113
+
t114
+
t115
+
t116
+
t117
;
118
float
t119
=
t2
*
t59
*
t118
;
119
float
t120
=
P
[4][6]*
t2
*
t57
;
120
float
t121
=
P
[5][6]*
t2
*
t59
;
121
float
t122
=
P
[6][6]*
t2
*
t42
;
122
float
t123
=
P
[0][6]*
t2
*
t46
;
123
float
t124
=
P
[1][6]*
t2
*
t49
;
124
float
t125
=
P
[2][6]*
t2
*
t52
;
125
float
t126
=
P
[3][6]*
t2
*
t55
;
126
float
t127
=
t120
+
t121
+
t122
+
t123
+
t124
+
t125
+
t126
;
127
float
t128
=
t2
*
t42
*
t127
;
128
float
t129
= R_LOS+
t75
+
t83
+
t92
+
t101
+
t110
+
t119
+
t128
;
129
float
t130
= 1.0f/
t129
;
130
H_LOS
[0] =
t2
*
t46
;
131
H_LOS
[1] =
t2
*
t49
;
132
H_LOS
[2] =
t2
*
t52
;
133
H_LOS
[3] =
t2
*
t55
;
134
H_LOS
[4] =
t2
*(
t28
+
t31
-Tbs.b.y*(
t23
-q1*q2*2.0));
135
H_LOS
[5] =
t2
*(
t33
+
t35
-Tbs.b.z*(
t26
-q2*q3*2.0));
136
H_LOS
[6] =
t2
*
t42
;
137
Kfusion
[0] =
t130
*(
t61
+
P
[0][6]*
t2
*
t42
+
P
[0][1]*
t2
*
t49
+
P
[0][2]*
t2
*
t52
+
P
[0][3]*
t2
*
t55
+
P
[0][4]*
t2
*
t57
+
P
[0][5]*
t2
*
t59
);
138
Kfusion
[1] =
t130
*(
t67
+
P
[1][0]*
t2
*
t46
+
P
[1][6]*
t2
*
t42
+
P
[1][2]*
t2
*
t52
+
P
[1][3]*
t2
*
t55
+
P
[1][4]*
t2
*
t57
+
P
[1][5]*
t2
*
t59
);
139
Kfusion
[2] =
t130
*(
t89
+
P
[2][0]*
t2
*
t46
+
P
[2][6]*
t2
*
t42
+
P
[2][1]*
t2
*
t49
+
P
[2][3]*
t2
*
t55
+
P
[2][4]*
t2
*
t57
+
P
[2][5]*
t2
*
t59
);
140
Kfusion
[3] =
t130
*(
t99
+
P
[3][0]*
t2
*
t46
+
P
[3][6]*
t2
*
t42
+
P
[3][1]*
t2
*
t49
+
P
[3][2]*
t2
*
t52
+
P
[3][4]*
t2
*
t57
+
P
[3][5]*
t2
*
t59
);
141
Kfusion
[4] =
t130
*(
t102
+
P
[4][0]*
t2
*
t46
+
P
[4][6]*
t2
*
t42
+
P
[4][1]*
t2
*
t49
+
P
[4][2]*
t2
*
t52
+
P
[4][3]*
t2
*
t55
+
P
[4][5]*
t2
*
t59
);
142
Kfusion
[5] =
t130
*(
t112
+
P
[5][0]*
t2
*
t46
+
P
[5][6]*
t2
*
t42
+
P
[5][1]*
t2
*
t49
+
P
[5][2]*
t2
*
t52
+
P
[5][3]*
t2
*
t55
+
P
[5][4]*
t2
*
t57
);
143
Kfusion
[6] =
t130
*(
t122
+
P
[6][0]*
t2
*
t46
+
P
[6][1]*
t2
*
t49
+
P
[6][2]*
t2
*
t52
+
P
[6][3]*
t2
*
t55
+
P
[6][4]*
t2
*
t57
+
P
[6][5]*
t2
*
t59
);
144
Kfusion
[7] =
t130
*(
P
[7][0]*
t2
*
t46
+
P
[7][6]*
t2
*
t42
+
P
[7][1]*
t2
*
t49
+
P
[7][2]*
t2
*
t52
+
P
[7][3]*
t2
*
t55
+
P
[7][4]*
t2
*
t57
+
P
[7][5]*
t2
*
t59
);
145
Kfusion
[8] =
t130
*(
P
[8][0]*
t2
*
t46
+
P
[8][6]*
t2
*
t42
+
P
[8][1]*
t2
*
t49
+
P
[8][2]*
t2
*
t52
+
P
[8][3]*
t2
*
t55
+
P
[8][4]*
t2
*
t57
+
P
[8][5]*
t2
*
t59
);
146
Kfusion
[9] =
t130
*(
P
[9][0]*
t2
*
t46
+
P
[9][6]*
t2
*
t42
+
P
[9][1]*
t2
*
t49
+
P
[9][2]*
t2
*
t52
+
P
[9][3]*
t2
*
t55
+
P
[9][4]*
t2
*
t57
+
P
[9][5]*
t2
*
t59
);
147
Kfusion
[10] =
t130
*(
P
[10][0]*
t2
*
t46
+
P
[10][6]*
t2
*
t42
+
P
[10][1]*
t2
*
t49
+
P
[10][2]*
t2
*
t52
+
P
[10][3]*
t2
*
t55
+
P
[10][4]*
t2
*
t57
+
P
[10][5]*
t2
*
t59
);
148
Kfusion
[11] =
t130
*(
P
[11][0]*
t2
*
t46
+
P
[11][6]*
t2
*
t42
+
P
[11][1]*
t2
*
t49
+
P
[11][2]*
t2
*
t52
+
P
[11][3]*
t2
*
t55
+
P
[11][4]*
t2
*
t57
+
P
[11][5]*
t2
*
t59
);
149
Kfusion
[12] =
t130
*(
P
[12][0]*
t2
*
t46
+
P
[12][6]*
t2
*
t42
+
P
[12][1]*
t2
*
t49
+
P
[12][2]*
t2
*
t52
+
P
[12][3]*
t2
*
t55
+
P
[12][4]*
t2
*
t57
+
P
[12][5]*
t2
*
t59
);
150
Kfusion
[13] =
t130
*(
P
[13][0]*
t2
*
t46
+
P
[13][6]*
t2
*
t42
+
P
[13][1]*
t2
*
t49
+
P
[13][2]*
t2
*
t52
+
P
[13][3]*
t2
*
t55
+
P
[13][4]*
t2
*
t57
+
P
[13][5]*
t2
*
t59
);
151
Kfusion
[14] =
t130
*(
P
[14][0]*
t2
*
t46
+
P
[14][6]*
t2
*
t42
+
P
[14][1]*
t2
*
t49
+
P
[14][2]*
t2
*
t52
+
P
[14][3]*
t2
*
t55
+
P
[14][4]*
t2
*
t57
+
P
[14][5]*
t2
*
t59
);
152
Kfusion
[15] =
t130
*(
P
[15][0]*
t2
*
t46
+
P
[15][6]*
t2
*
t42
+
P
[15][1]*
t2
*
t49
+
P
[15][2]*
t2
*
t52
+
P
[15][3]*
t2
*
t55
+
P
[15][4]*
t2
*
t57
+
P
[15][5]*
t2
*
t59
);
153
Kfusion
[16] =
t130
*(
P
[16][0]*
t2
*
t46
+
P
[16][6]*
t2
*
t42
+
P
[16][1]*
t2
*
t49
+
P
[16][2]*
t2
*
t52
+
P
[16][3]*
t2
*
t55
+
P
[16][4]*
t2
*
t57
+
P
[16][5]*
t2
*
t59
);
154
Kfusion
[17] =
t130
*(
P
[17][0]*
t2
*
t46
+
P
[17][6]*
t2
*
t42
+
P
[17][1]*
t2
*
t49
+
P
[17][2]*
t2
*
t52
+
P
[17][3]*
t2
*
t55
+
P
[17][4]*
t2
*
t57
+
P
[17][5]*
t2
*
t59
);
155
Kfusion
[18] =
t130
*(
P
[18][0]*
t2
*
t46
+
P
[18][6]*
t2
*
t42
+
P
[18][1]*
t2
*
t49
+
P
[18][2]*
t2
*
t52
+
P
[18][3]*
t2
*
t55
+
P
[18][4]*
t2
*
t57
+
P
[18][5]*
t2
*
t59
);
156
Kfusion
[19] =
t130
*(
P
[19][0]*
t2
*
t46
+
P
[19][6]*
t2
*
t42
+
P
[19][1]*
t2
*
t49
+
P
[19][2]*
t2
*
t52
+
P
[19][3]*
t2
*
t55
+
P
[19][4]*
t2
*
t57
+
P
[19][5]*
t2
*
t59
);
157
Kfusion
[20] =
t130
*(
P
[20][0]*
t2
*
t46
+
P
[20][6]*
t2
*
t42
+
P
[20][1]*
t2
*
t49
+
P
[20][2]*
t2
*
t52
+
P
[20][3]*
t2
*
t55
+
P
[20][4]*
t2
*
t57
+
P
[20][5]*
t2
*
t59
);
158
Kfusion
[21] =
t130
*(
P
[21][0]*
t2
*
t46
+
P
[21][6]*
t2
*
t42
+
P
[21][1]*
t2
*
t49
+
P
[21][2]*
t2
*
t52
+
P
[21][3]*
t2
*
t55
+
P
[21][4]*
t2
*
t57
+
P
[21][5]*
t2
*
t59
);
159
Kfusion
[22] =
t130
*(
P
[22][0]*
t2
*
t46
+
P
[22][6]*
t2
*
t42
+
P
[22][1]*
t2
*
t49
+
P
[22][2]*
t2
*
t52
+
P
[22][3]*
t2
*
t55
+
P
[22][4]*
t2
*
t57
+
P
[22][5]*
t2
*
t59
);
160
Kfusion
[23] =
t130
*(
P
[23][0]*
t2
*
t46
+
P
[23][6]*
t2
*
t42
+
P
[23][1]*
t2
*
t49
+
P
[23][2]*
t2
*
t52
+
P
[23][3]*
t2
*
t55
+
P
[23][4]*
t2
*
t57
+
P
[23][5]*
t2
*
t59
);
t5
float t5
Definition:
LOSX.c:5
t74
float t74
Definition:
LOSX.c:73
t43
float t43
Definition:
LOSX.c:43
t23
float t23
Definition:
LOSX.c:22
t13
float t13
Definition:
LOSX.c:15
t85
float t85
Definition:
LOSX.c:84
t44
float t44
Definition:
LOSX.c:44
t17
float t17
Definition:
LOSX.c:12
t7
float t7
Definition:
LOSX.c:7
t111
float t111
Definition:
LOSX.c:110
t75
float t75
Definition:
LOSX.c:74
t120
float t120
Definition:
LOSX.c:119
t35
float t35
Definition:
LOSX.c:34
t101
float t101
Definition:
LOSX.c:100
t102
float t102
Definition:
LOSX.c:101
t129
float t129
Definition:
LOSX.c:128
t71
float t71
Definition:
LOSX.c:70
t29
float t29
Definition:
LOSX.c:28
t123
float t123
Definition:
LOSX.c:122
t18
float t18
Definition:
LOSX.c:4
t21
float t21
Definition:
LOSX.c:20
t92
float t92
Definition:
LOSX.c:91
t87
float t87
Definition:
LOSX.c:86
t27
float t27
Definition:
LOSX.c:26
t99
float t99
Definition:
LOSX.c:98
t32
float t32
Definition:
LOSX.c:31
t103
float t103
Definition:
LOSX.c:102
t98
float t98
Definition:
LOSX.c:97
t4
float t4
Definition:
LOSX.c:3
t50
float t50
Definition:
LOSX.c:51
t2
float t2
Definition:
LOSX.c:1
t12
float t12
Definition:
LOSX.c:14
t82
float t82
Definition:
LOSX.c:81
t16
float t16
Definition:
LOSX.c:8
t78
float t78
Definition:
LOSX.c:77
H_LOS
H_LOS[0]
Definition:
LOSX.c:130
t26
float t26
Definition:
LOSX.c:25
t60
float t60
Definition:
LOSX.c:41
t77
float t77
Definition:
LOSX.c:76
t14
float t14
Definition:
LOSX.c:16
t90
float t90
Definition:
LOSX.c:89
t40
float t40
Definition:
LOSX.c:39
t121
float t121
Definition:
LOSX.c:120
t56
float t56
Definition:
LOSX.c:59
t73
float t73
Definition:
LOSX.c:72
t113
float t113
Definition:
LOSX.c:112
t42
float t42
Definition:
LOSX.c:42
t41
float t41
Definition:
LOSX.c:40
t38
float t38
Definition:
LOSX.c:37
t62
float t62
Definition:
LOSX.c:49
t125
float t125
Definition:
LOSX.c:124
t37
float t37
Definition:
LOSX.c:36
t72
float t72
Definition:
LOSX.c:71
t112
float t112
Definition:
LOSX.c:111
t51
float t51
Definition:
LOSX.c:52
Kfusion
Kfusion[0]
Definition:
LOSX.c:137
t58
float t58
Definition:
LOSX.c:62
t3
float t3
Definition:
LOSX.c:2
t104
float t104
Definition:
LOSX.c:103
t94
float t94
Definition:
LOSX.c:93
t31
float t31
Definition:
LOSX.c:30
t45
float t45
Definition:
LOSX.c:45
t48
float t48
Definition:
LOSX.c:48
t20
float t20
Definition:
LOSX.c:19
t114
float t114
Definition:
LOSX.c:113
t59
float t59
Definition:
LOSX.c:64
t67
float t67
Definition:
LOSX.c:66
t106
float t106
Definition:
LOSX.c:105
t22
float t22
Definition:
LOSX.c:21
t100
float t100
Definition:
LOSX.c:99
t55
float t55
Definition:
LOSX.c:58
t105
float t105
Definition:
LOSX.c:104
t96
float t96
Definition:
LOSX.c:95
t126
float t126
Definition:
LOSX.c:125
t15
float t15
Definition:
LOSX.c:17
t86
float t86
Definition:
LOSX.c:85
t95
float t95
Definition:
LOSX.c:94
t97
float t97
Definition:
LOSX.c:96
t25
float t25
Definition:
LOSX.c:24
t57
float t57
Definition:
LOSX.c:61
t34
float t34
Definition:
LOSX.c:33
t8
float t8
Definition:
LOSX.c:9
t89
float t89
Definition:
LOSX.c:88
t53
float t53
Definition:
LOSX.c:55
t130
float t130
Definition:
LOSX.c:129
t30
float t30
Definition:
LOSX.c:29
t19
float t19
Definition:
LOSX.c:18
t65
float t65
Definition:
LOSX.c:60
t110
float t110
Definition:
LOSX.c:109
t69
float t69
Definition:
LOSX.c:68
t61
float t61
Definition:
LOSX.c:65
t47
float t47
Definition:
LOSX.c:47
t128
float t128
Definition:
LOSX.c:127
t9
float t9
Definition:
LOSX.c:10
t63
float t63
Definition:
LOSX.c:53
t109
float t109
Definition:
LOSX.c:108
t52
float t52
Definition:
LOSX.c:54
t115
float t115
Definition:
LOSX.c:114
t46
float t46
Definition:
LOSX.c:46
t66
float t66
Definition:
LOSX.c:63
t68
float t68
Definition:
LOSX.c:67
t88
float t88
Definition:
LOSX.c:87
t80
float t80
Definition:
LOSX.c:79
t54
float t54
Definition:
LOSX.c:56
t107
float t107
Definition:
LOSX.c:106
t116
float t116
Definition:
LOSX.c:115
t10
float t10
Definition:
LOSX.c:11
t84
float t84
Definition:
LOSX.c:83
t76
float t76
Definition:
LOSX.c:75
P
P[0][0]
Definition:
quatCovMat.c:44
t64
float t64
Definition:
LOSX.c:57
t124
float t124
Definition:
LOSX.c:123
t108
float t108
Definition:
LOSX.c:107
t122
float t122
Definition:
LOSX.c:121
t117
float t117
Definition:
LOSX.c:116
t79
float t79
Definition:
LOSX.c:78
t28
float t28
Definition:
LOSX.c:27
t91
float t91
Definition:
LOSX.c:90
t24
float t24
Definition:
LOSX.c:23
t49
float t49
Definition:
LOSX.c:50
t6
float t6
Definition:
LOSX.c:6
t81
float t81
Definition:
LOSX.c:80
t39
float t39
Definition:
LOSX.c:38
t118
float t118
Definition:
LOSX.c:117
t70
float t70
Definition:
LOSX.c:69
t119
float t119
Definition:
LOSX.c:118
t33
float t33
Definition:
LOSX.c:32
t11
float t11
Definition:
LOSX.c:13
t93
float t93
Definition:
LOSX.c:92
t36
float t36
Definition:
LOSX.c:35
t127
float t127
Definition:
LOSX.c:126
t83
float t83
Definition:
LOSX.c:82
src
lib
ecl
EKF
matlab
scripts
Inertial Nav EKF
LOSX.c
Generated by
1.8.13