-
Notifications
You must be signed in to change notification settings - Fork 1.1k
/
ins_float.c
234 lines (189 loc) · 4.79 KB
/
ins_float.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
/*
* Paparazzi autopilot $Id$
*
* Copyright (C) 2004-2010 The Paparazzi Team
*
* This file is part of paparazzi.
*
* paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option)
* any later version.
*
* paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with paparazzi; see the file COPYING. If not, write to
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*
*/
/** \file estimator.c
* \brief State estimate, fusioning sensors
*/
#include "subsystems/ins/ins_float.h"
#include <inttypes.h>
#include <math.h>
#include "state.h"
#include "mcu_periph/uart.h"
#include "ap_downlink.h"
#include "subsystems/gps.h"
#include "subsystems/nav.h"
/* vertical position and speed in meters */
float estimator_z;
float estimator_z_dot;
#if USE_BAROMETER
#include "subsystems/sensors/baro.h"
int32_t ins_qfe;
bool_t ins_baro_initialised;
float ins_baro_alt;
#endif
bool_t ins_hf_realign;
bool_t ins_vf_realign;
void ins_init() {
struct UtmCoor_f utm0 = { nav_utm_north0, nav_utm_east0, 0., nav_utm_zone0 };
stateSetLocalUtmOrigin_f(&utm0);
stateSetPositionUtm_f(&utm0);
#ifdef ALT_KALMAN
alt_kalman_init();
#endif
#if USE_BAROMETER
ins_qfe = 0;;
ins_baro_initialised = FALSE;
ins_baro_alt = 0.;
#endif
ins_vf_realign = FALSE;
EstimatorSetAlt(0.);
}
void ins_periodic( void ) {
}
void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {
}
void ins_realign_v(float z __attribute__ ((unused))) {
}
void ins_propagate() {
}
void ins_update_baro() {
#if USE_BAROMETER
// TODO update kalman filter with baro struct
if (baro.status == BS_RUNNING) {
if (!ins_baro_initialised) {
ins_qfe = baro.absolute;
ins_baro_initialised = TRUE;
}
if (ins_vf_realign) {
ins_vf_realign = FALSE;
ins_qfe = baro.absolute;
}
else { /* not realigning, so normal update with baro measurement */
ins_baro_alt = (baro.absolute - ins_qfe) * INS_BARO_SENS;
EstimatorSetAlt(ins_baro_alt);
}
}
#endif
}
void ins_update_gps(void) {
#if USE_GPS
struct UtmCoor_f utm;
utm.east = gps.utm_pos.east / 100.;
utm.north = gps.utm_pos.north / 100.;
utm.zone = nav_utm_zone0;
#if !USE_BAROMETER
float falt = gps.hmsl / 1000.;
EstimatorSetAlt(falt);
#endif
utm.alt = estimator_z;
// set position
stateSetPositionUtm_f(&utm);
struct NedCoor_f ned_vel = {
gps.ned_vel.x / 100.,
gps.ned_vel.y / 100.,
gps.ned_vel.z / 100.
};
// set velocity
stateSetSpeedNed_f(&ned_vel);
#endif
}
void ins_update_sonar() {
}
bool_t alt_kalman_enabled;
#ifdef ALT_KALMAN
#ifndef ALT_KALMAN_ENABLED
#define ALT_KALMAN_ENABLED FALSE
#endif
#define GPS_SIGMA2 1.
#define GPS_DT 0.25
#define GPS_R 2.
#define BARO_DT 0.1
static float p[2][2];
void alt_kalman_reset( void ) {
p[0][0] = 1.;
p[0][1] = 0.;
p[1][0] = 0.;
p[1][1] = 1.;
}
void alt_kalman_init( void ) {
alt_kalman_enabled = ALT_KALMAN_ENABLED;
alt_kalman_reset();
}
void alt_kalman(float z_meas) {
float DT;
float R;
float SIGMA2;
#if USE_BARO_MS5534A
if (alt_baro_enabled) {
DT = BARO_DT;
R = baro_MS5534A_r;
SIGMA2 = baro_MS5534A_sigma2;
} else
#elif USE_BARO_ETS
if (baro_ets_enabled) {
DT = BARO_ETS_DT;
R = baro_ets_r;
SIGMA2 = baro_ets_sigma2;
} else
#elif USE_BARO_BMP
if (baro_bmp_enabled) {
DT = BARO_BMP_DT;
R = baro_bmp_r;
SIGMA2 = baro_bmp_sigma2;
} else
#endif
{
DT = GPS_DT;
R = GPS_R;
SIGMA2 = GPS_SIGMA2;
}
float q[2][2];
q[0][0] = DT*DT*DT*DT/4.;
q[0][1] = DT*DT*DT/2.;
q[1][0] = DT*DT*DT/2.;
q[1][1] = DT*DT;
/* predict */
estimator_z += estimator_z_dot * DT;
p[0][0] = p[0][0]+p[1][0]*DT+DT*(p[0][1]+p[1][1]*DT) + SIGMA2*q[0][0];
p[0][1] = p[0][1]+p[1][1]*DT + SIGMA2*q[0][1];
p[1][0] = p[1][0]+p[1][1]*DT + SIGMA2*q[1][0];
p[1][1] = p[1][1] + SIGMA2*q[1][1];
/* error estimate */
float e = p[0][0] + R;
if (fabs(e) > 1e-5) {
float k_0 = p[0][0] / e;
float k_1 = p[1][0] / e;
e = z_meas - estimator_z;
/* correction */
estimator_z += k_0 * e;
estimator_z_dot += k_1 * e;
p[1][0] = -p[0][0]*k_1+p[1][0];
p[1][1] = -p[0][1]*k_1+p[1][1];
p[0][0] = p[0][0] * (1-k_0);
p[0][1] = p[0][1] * (1-k_0);
}
#ifdef DEBUG_ALT_KALMAN
DOWNLINK_SEND_ALT_KALMAN(DefaultChannel,DefaultDevice,&(p[0][0]),&(p[0][1]),&(p[1][0]), &(p[1][1]));
#endif
}
#endif // ALT_KALMAN