hdgps.txt

无人机高度基于matlab卡尔曼滤波程序 数据融合

-2000
-2000
-2000
1
1
1
2
2
2
1
1
1
0
0
0
1
1
1
1
0
0
1
1
1
2
1
1
1
1
1
2
2
2
2
2
2
3
2
2
2
2
2
2
3
3
3
3
3
4
5
5
4
4
4
5
4
4
4
4
4
4
5
5
4
4
5
3
3
3
3
3
3
2
3
3
4
4
4
4
4
4
4
4
4
5
5
5
6
6
5
5
6
6
5
5
5
5
4
4
5
5
5
5
5
5
4
4
5
4
5
5
5
5
4
4
4
4
4
4
4
5
4
4
4
4
4
4
4
4
5
5
5
5
6
6
5
5
5
5
4
4
5
5
5
4
4
4
3
3
4
4
4
4
4
4
3
4
3
3
3
3
4
3
2
2
2
2
2
1
2
2
2
2
2
3
2
2
3
3
2
2
3
3
2
2
3
3
3
3
5
5
4
4
4
4
3
3
3
3
4
4
4
4
4
4
4
4
4
4
4
3
3
3
3
3
5
4
3
3
3
3
3
3
3
3
4
4
3
3
2
2
3
3
2
3
3
3
3
3
3
4
3
3
2
2
3
2
3
3
4
4
4
4
3
3
4
4
4
4
4
4
3
3
2
2
3
3
3
3
2
4
5
5
4
4
3
3
4
4
3
3
2
2
2
2
2
2
1
1
1
1
2
2
3
0
1
1
2
2
1
2
2
2
2
2
1
3
0
0
0
0
3
4
2
2
3
3
4
5
3
3
3
3
3
3
2
2
2
2
3
2
2
2
3
3
3
3
2
2
3
3
3
4
5
5
3
3
1
4
3
3
3
3
3
4
5
5
2
2
2
5
5
5
6
6
5
3
4
4
5
5
5
3
4
4
3
3
3
4
3
3
4
4
1
3
3
3
4
4
4
3
3
3
5
5
2
3
1
1
2
2
3
0
2
2
2
2
2
2
2
2
3
3
2
1
3
3
3
3
5
4
3
3
3
3
4
3
4
4
4
4
5
4
4
4
5
5
5
7
4
4
6
6
4
4
2
2
3
3
3
3
2
2
2
2
0
2
2
2
1
1
1
1
0
0
1
1
1
0
2
2
2
2
0
1
3
3
2
2
2
2
2
2
2
2
2
2
2
2
2
2
1
1
2
2
2
2
0
0
2
2
4
4
1
2
2
2
2
2
1
2
4
4
4
4
1
2
4
4
4
4
3
3
6
6
6
6
4
2
3
3
2
2
1
2
2
2
2
2
2
5
-1
-1
0
0
3
2
2
2
3
3
0
1
1
1
0
0
1
0
2
2
2
2
1
1
2
2
2
2
2
2
-1
-1
-1
-1
1
2
2
2
3
3
2
1
2
2
2
2
1
3
1
1
1
1
1
2
2
2
3
3
0
0
3
3
4
4
2
4
4
4
5
5
3
3
4
4
4
4
4
3
4
4
4
4
4
5
3
3
5
5
5
5
5
5
6
6
5
5
4
4
5
5
5
3
1
1
1
1
3
1
3
3
4
4
3
1
2
2
3
3
3
4
3
3
4
4
4
4
4
4
4
4
4
5
4
4
4
4
5
5
5
5
5
5
4
3
3
3
3
3
2
3
3
3
3
3
4
2
2
2
2
2
3
4
4
4
3
3
3
4
3
3
2
2
2
3
3
3
2
2
2
4
4
4
4
4
4
5
5
5
4
4
5
4
4
4
4
4
4
5
6
6
4
4
5
4
4
4
5
5
5
5
4
4
4
4
3
4
4
4
4
4
4
2
4
4
3
3
2
2
3
3
3
3
4
3
4
4
4
4
3
4
4
4
4
4
4
4
3
3
3
3
3
4
3
3
3
3
2
2
2
2
3
3
3
2
3
3
2
2
3
2
2
2
2
2
1
1
2
2
2
2
3
3
2
2
2
2
2
3
4
4
3
3
3
5
2
2
4
4
3
4
3
3
3
3
2
3
4
4
4
4
3
4
3
3
3
3
4
3
3
3
4
4
4
3
3
3
4
4
2
1
1
1
2
2
2
3
3
3
2
2
-1
1
0
0
1
1
0
0
1
1
0
0
0
0
0
0
0
0
0
1
0
0
0
0
1
0
0
0
0
0
0
-1
-1
-1
-1
-1
0
0
0
0
0
0
0
1
0
0
0
0
1
2
1
1
1
1
1
1
0
0
1
1
0
1
1
1
1
1
1
0
0
0
0
0
0
1
1
1
1
1
0
1
1
1
0
0
-1
0
0
0
0
0
1
1
0
0
0
0
1
1
0
0
0
0
1
1
1
1
1
1
0
1
2
2
1
1
1
2
2
2
3
3
2
2
2
2
3
3
3
3
2
2
3
3
4
4
3
3
3
3
2
3
3
3
4
4
3
3
3
3
3
3
3
4
5
5
5
5
5
4
4
4
3
3
4
3
3
3
4
4
3
4
4
4
3
3
3
3
3
3
5
5
5
4
5
5
5
5
6
5
5
5
6
6
6
7
8
8
8
8
9
9
9
9
10
10
9
11
12
12
11
11
12
12
13
13
14
14
15
15
15
15
15
15
16
16
16
16
17
17
17
18
19
19
19
19
19
20
20
20
20
20
21
20
21
21
23
23
22
23
25
25
25
25
25
25
26
26
26
26
27
26
28
28
28
28
29
30
31
31
32
32
32
32
33
33
34
34
36
37
40
40
41
41
42
43
44
44
45
45
46
47
48
48
49
49
50
50
52
52
52
52
53
54
56
56
57
57
58
59
59
59
60
60
60
61
62
62
63
63
63
64
64
64
64
64
64
66
66
66
68
68
68
69
70
70
70
70
70
71
70
70
72
72
73
73
73
73
73
73
72
74
73
73
73
73
74
74
75
75
75
75
75
76
77
77
78
78
78
78
79
79
80
80
81
81
82
82
82
82
83
83
84
84
84
84
84
85
85
85
86
86
86
86
86
86
87
87
88
88
88
88
87
87
87
88
88
88
89
89
90
89
89
89
89
89
90
89
90
90
91
91
90
90
91
91
91
91
91
91
92
92
92
92
92
92
93
93
93
93
92
94
95
95
95
95
97
96
98
98
98
98
100
100
101
101
101
101
101
101
100
100
101
101
102
101
101
101
101
101
100
99
100
100
99
99
98
98
98
98
98
98
96
96
95
95
96
96
96
95
95
95
95
95
96
95
95
95
96
96
96
96
97
97
97
97
98
99
99
99
99
99
101
100
101
101
101
101
102
103
104
104
104
104
105
106
107
107
107
107
107
108
108
108
109
109
108
107
107
107
108
108
107
108
108
108
107
107
107
106
106
106
107
107
108
107
106
106
106
106
107
106
107
107
106
106
106
106
106
106
105
105
106
106
105
105
105
105
104
103
103
103
103
103
102
101
101
101
101
101
101
100
100
100
99
99
100
100
99
99
99
99
99
99
100
100
99
99
99
99
99
99
100
100
98
99
99
99
100
100
99
99
99
99
100
100
100
99
99
99
99
99
99
99
99
99
98
98
99
99
99
99
99
99
99
99
99
99
99
99
100
100
100
100
99
99
99
97
98
98
98
98
97
97
96
96
96
96
96
94
94
94
94
94
93
93
94
94
93
93
94
94
94
94
95
95
95
96
95
95
96
96
95
96
96
96
96
96
96
95
96
96
96
96
96
96
96
96
96
96
95
96
96
96
96
96
96
97
96
96
96
96
97
96
97
97
97
97
97
97
98
98
99
99
99
99
99
99
100
100
100
100
100
100
100
100
101
101
102
102
102
102
103
104
105
105
105
105
105
108
110
110
111
111
111
112
113
113
114
114
114
115
115
115
115
115
117
118
119
119
119
119
120
119
122
122
123
123
122
123
124
124
125
125
124
125
126
126
127
127
126
125
126
126
127
127
127
126
126
126
126
126
126
126
126
126
126
126
126
127
128
128
127
127
128
128
129
129
129
129
130
130
131
131
130
130
130
129
131
131
131
131
131
132
133
133
132
132
134
134
134
134
135
135
137
138
138
138
138
138
139
139
140
140
140
140
140
141
142
142
142
142
143
143
143
143
144
144
145
145
146
146
147
147
147
147
149
149
148
148
148
148
148
148
149
149
149
150
150
150
151
151
153
153
155
155
154
154
157
156
157
157
158
158
158
159
158
158
160
160
161
161
162
162
164
164
164
164
165
165
164
164
167
167
168
168
169
169
167
166
167
167
168
168
171
171
171
171
171
171
170
169
169
169
171
171
170
172
172
172
172
172
174
174
176
176
175
175
177
177
176
176
177
177
176
177
178
178
179
179
178
178
179
179
179
179
183
182
181
181
183
183
182
183
184
184
183
183
184
185
184
184
184
184
185
186
186
186
186
186
185
185
186
186
185
185
187
187
189
189
188
188
188
188
188
188
189
189
189
191
190
190
190
190
188
191
192
192
191
191
192
191
190
190
190
190
191
191
193
193
193
193
194
194
193
193
195
195
194
193
193
193
192
192
192
195
193
193
193
193
193
193
194
194
194
194
192
193
192
192
191
191
193
196
193
193
194
194
191
193
191
191
193
193
192
192
193
193
193
193
193
194
193
193
194
194
193
193
195
195
195
195
195
197
196
196
197
197
198
199
198
198
197
197
198
197
197
197
198
198
197
201
200
200
199
199
199
199
201
201
200
200
200
200
201
201
201
201
200
202
202
202
202
202
202
203
201
201
201
201
201
201
202
202
203
203
200
203
203
203
201
201
201
201
199
199
200
200
202
200
201
201
201
201
201
202
201
201
201
201
202
202
204
204
204
204
201
204
203
203
203
203
204
203
203
203
203
203
204
204
205
205
205
205
204
206
205
205
205
205
204
204
205
205
204
204
205
205
204
204
204
204
204
203
204
204
204
204
206
206
206
206
205
205
205
208
208
208
207
207
208
210
209
209
210
210
211
210
209
209
209
209
209
210
209
209
210
210
212
210
213
213
211
211
213
213
212
212
211
211
212
212
214
214
213
213
213
213
213
213
214
214
214
215
215
215
214
214
215
213
214
214
213
213
214
209
214
214
212
212
211
212
209
209
210
210
209
210
212
212
212
212
210
212
211
211
210
210
210
210
211
211
209
209
207
208
208
208
208
208
207
206
206
206
206
206
208
207
204
204
205
205
207
206
207
207
206
206
204
203
206
206
204
204
203
202
202
202
203
203
202
201
200
200
202
202
200
199
199
199
199
199
199
198
199
199
201
201
199
199
198
198
200
200
198
199
203
203
200
200
200
201
202
202
201
201
202
199
201
201
200
200
199
200
198
198
198
198
199
198
199
199
198
198
198
200
198
198
198
198
200
201
201
201
199
199
198
199
198
198
199
199
199
200
200
200
199
199
198
198
199
199
199
199
199
198
201
201
200
200
201
200
200
200
202
202
202
203
202
202
204
204
203
204
205
205
205
205
206
204
205
205
204
204
202
204
204
204
203
203
203
201
202
202
201
201
199
200
201
201
201
201
201
201
201
201
200
200
202
201
201
201
203
203
202
201
200
200
201
201
199
200
199
199
199
199
200
200
199
199
200
200
199
199
200
200
199
199
199
199
199
199
198
198
199
198
199
199
199
199
197
198
197
197
197
197
197
198
198
198
198
198
197
199
199
199
199
199
198
198
198
198
199
199
198
200
200
200
199
199
199
197
199
199
198
198
199
199
199
199
199
199
199
199
199
199
201
201
200
200
199
199
199
199
201
200
200
200
202
202
200
201
202
202
202
202
201
202
201
201
201
201
201
200
200
200
201
201
199
200
198
198
199
199
199
197
199
199
198
198
197
196
196
196
197
197
199
198
197
197
197
197
198
199
198
198
197
197
197
196
197
197
198
198
197
196
198
198
197
197
196
197
197
197
197
197
197
198
199
199
197
197
197
197
197
197
198
198
199
198
198
198
197
197
198
197
198
198
197
197
196
198
198
198
198
198
198
196
197
197
197
197
197
197
198
198
198
198
198
198
200
200
199
199
199
201
201
201
202
202
202
201
203
203
202
202
201
202
202
202
203
203
203
203
202
202
203
203
203
202
202
202
201
201
202
203
204
204
200
200
200
200
201
201
201
201
200
200
200
200
200
200
201
199
200
200
199
199
200
198
199
199
198
198
198
198
199
199
198
198
197
198
198
198
197
197
197
198
198
198
198
198
198
197
198
198
201
201
198
196
199