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
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
|
#pragma once
#include "estimator_utilities.h"
const unsigned int n_states = 23;
const unsigned int data_buffer_size = 50;
class AttPosEKF {
public:
AttPosEKF();
~AttPosEKF();
/* ##############################################
*
* M A I N F I L T E R P A R A M E T E R S
*
* ########################################### */
/*
* parameters are defined here and initialised in
* the InitialiseParameters() (which is just 20 lines down)
*/
float covTimeStepMax; // maximum time allowed between covariance predictions
float covDelAngMax; // maximum delta angle between covariance predictions
float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
float a1; // optical flow sensor misalgnment angle about X axis (rad)
float a2; // optical flow sensor misalgnment angle about Y axis (rad)
float a3; // optical flow sensor misalgnment angle about Z axis (rad)
float yawVarScale;
float windVelSigma;
float dAngBiasSigma;
float dVelBiasSigma;
float magEarthSigma;
float magBodySigma;
float gndHgtSigma;
float vneSigma;
float vdSigma;
float posNeSigma;
float posDSigma;
float magMeasurementSigma;
float airspeedMeasurementSigma;
float gyroProcessNoise;
float accelProcessNoise;
float EAS2TAS; // ratio f true to equivalent airspeed
void InitialiseParameters()
{
covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions
covDelAngMax = 0.02f; // maximum delta angle between covariance predictions
rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis.
EAS2TAS = 1.0f;
a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad)
a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad)
a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad)
yawVarScale = 1.0f;
windVelSigma = 0.1f;
dAngBiasSigma = 5.0e-7f;
dVelBiasSigma = 1e-4f;
magEarthSigma = 3.0e-4f;
magBodySigma = 3.0e-4f;
gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma
vneSigma = 0.2f;
vdSigma = 0.3f;
posNeSigma = 2.0f;
posDSigma = 2.0f;
magMeasurementSigma = 0.05;
airspeedMeasurementSigma = 1.4f;
gyroProcessNoise = 1.4544411e-2f;
accelProcessNoise = 0.5f;
gndHgtSigma = 0.1f; // terrain gradient 1-sigma
R_LOS = 0.03f; // optical flow measurement noise variance (rad/sec)^2
flowInnovGate = 3.0f; // number of standard deviations applied to the optical flow innovation consistency check
auxFlowInnovGate = 10.0f; // number of standard deviations applied to the optical flow innovation consistency check used by the auxiliary filter
rngInnovGate = 5.0f; // number of standard deviations applied to the rnage finder innovation consistency check
minFlowRng = 0.01f; //minimum range between ground and flow sensor
moCompR_LOS = 0.2; // scaler from sensor gyro rate to uncertainty in LOS rate
}
struct mag_state_struct {
unsigned obsIndex;
float MagPred[3];
float SH_MAG[9];
float q0;
float q1;
float q2;
float q3;
float magN;
float magE;
float magD;
float magXbias;
float magYbias;
float magZbias;
float R_MAG;
Mat3f DCM;
};
struct mag_state_struct magstate;
struct mag_state_struct resetMagState;
// Global variables
float KH[n_states][n_states]; // intermediate result used for covariance updates
float KHP[n_states][n_states]; // intermediate result used for covariance updates
float P[n_states][n_states]; // covariance matrix
float Kfusion[n_states]; // Kalman gains
float states[n_states]; // state matrix
float resetStates[n_states];
float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps
uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored
// Times
uint64_t lastVelPosFusion; // the time of the last velocity fusion, in the standard time unit of the filter
float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements
float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement
float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time
float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time
float statesAtRngTime[n_states]; // filter states at the effective measurement time
float statesAtFlowTime[n_states]; // States at the effective optical flow measurement time
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s)
Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad)
Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s)
float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2)
Vector3f earthRateNED; // earths angular rate vector in NED (rad/s)
Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s)
Vector3f lastGyroOffset; // Last gyro offset
Vector3f delAngTotal;
Mat3f Tbn; // transformation matrix from body to NED coordinates
Mat3f Tnb; // transformation amtrix from NED to body coordinates
Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f dVelIMU;
Vector3f dAngIMU;
float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec), this may have significant jitter
float dtIMUfilt; // average time between IMU measurements (sec)
float dtVelPos; // time lapsed since the last position / velocity fusion (seconds), this may have significant jitter
float dtVelPosFilt; // average time between position / velocity fusion steps
float dtHgtFilt; // average time between height measurement updates
float dtGpsFilt; // average time between gps measurement updates
float windSpdFiltNorth; // average wind speed north component
float windSpdFiltEast; // average wind speed east component
float windSpdFiltAltitude; // the last altitude used to filter wind speed
float windSpdFiltClimb; // filtered climb rate
uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity
float innovVelPos[6]; // innovation output
float varInnovVelPos[6]; // innovation variance output
float velNED[3]; // North, East, Down velocity obs (m/s)
float accelGPSNED[3]; // Acceleration predicted by GPS in earth frame
float posNE[2]; // North, East position obs (m)
float hgtMea; // measured height (m)
float baroHgtOffset; ///< the baro (weather) offset from normalized altitude
float rngMea; // Ground distance
float innovMag[3]; // innovation output
float varInnovMag[3]; // innovation variance output
Vector3f magData; // magnetometer flux radings in X,Y,Z body axes
float losData[2]; // optical flow LOS rate measurements (rad/sec)
float innovVtas; // innovation output
float innovRng; ///< Range finder innovation
float innovOptFlow[2]; // optical flow LOS innovations (rad/sec)
float varInnovOptFlow[2]; // optical flow innovations variances (rad/sec)^2
float varInnovVtas; // innovation variance output
float varInnovRng; // range finder innovation variance
float VtasMeas; // true airspeed measurement (m/s)
float magDeclination; ///< magnetic declination
double latRef; // WGS-84 latitude of reference point (rad)
double lonRef; // WGS-84 longitude of reference point (rad)
float hgtRef; // WGS-84 height of reference point (m)
bool refSet; ///< flag to indicate if the reference position has been set
Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes
unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction
// GPS input data variables
double gpsLat;
double gpsLon;
float gpsHgt;
uint8_t GPSstatus;
// Baro input
float baroHgt;
bool statesInitialised;
bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused
bool fusePosData; // this boolean causes the posNE and velNED obs to be fused
bool fuseHgtData; // this boolean causes the hgtMea obs to be fused
bool fuseMagData; // boolean true when magnetometer data is to be fused
bool fuseVtasData; // boolean true when airspeed data is to be fused
bool fuseRngData; ///< true when range data is fused
bool fuseOptFlowData; // true when optical flow data is fused
bool inhibitWindStates; // true when wind states and covariances are to remain constant
bool inhibitMagStates; // true when magnetic field states and covariances are to remain constant
bool inhibitGndState; // true when the terrain ground height offset state and covariances are to remain constant
bool inhibitScaleState; // true when the focal length scale factor state and covariances are to remain constant
bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying)
bool staticMode; ///< boolean true if no position feedback is fused
bool useAirspeed; ///< boolean true if airspeed data is being used
bool useCompass; ///< boolean true if magnetometer data is being used
bool useRangeFinder; ///< true when rangefinder is being used
bool useOpticalFlow; // true when optical flow data is being used
bool ekfDiverged;
uint64_t lastReset;
struct ekf_status_report current_ekf_state;
struct ekf_status_report last_ekf_error;
bool numericalProtection;
unsigned storeIndex;
// Optical Flow error estimation
float storedOmega[3][data_buffer_size]; // angular rate vector stored for the last 50 time steps used by optical flow eror estimators
// Two state EKF used to estimate focal length scale factor and terrain position
float Popt[2][2]; // state covariance matrix
float flowStates[2]; // flow states [scale factor, terrain position]
float prevPosN; // north position at last measurement
float prevPosE; // east position at last measurement
float auxFlowObsInnov[2]; // optical flow observation innovations from focal length scale factor estimator
float auxFlowObsInnovVar[2]; // innovation variance for optical flow observations from focal length scale factor estimator
float fScaleFactorVar; // optical flow sensor focal length scale factor variance
Mat3f Tnb_flow; // Transformation matrix from nav to body at the time fo the optical flow measurement
float R_LOS; // Optical flow observation noise variance (rad/sec)^2
float auxFlowTestRatio[2]; // ratio of X and Y flow observation innovations to fault threshold
float auxRngTestRatio; // ratio of range observation innovations to fault threshold
float flowInnovGate; // number of standard deviations used for the innovation consistency check
float auxFlowInnovGate; // number of standard deviations applied to the optical flow innovation consistency check
float rngInnovGate; // number of standard deviations used for the innovation consistency check
float minFlowRng; // minimum range over which to fuse optical flow measurements
float moCompR_LOS; // scaler from sensor gyro rate to uncertainty in LOS rate
void updateDtGpsFilt(float dt);
void updateDtHgtFilt(float dt);
void UpdateStrapdownEquationsNED();
void CovariancePrediction(float dt);
void FuseVelposNED();
void FuseMagnetometer();
void FuseAirspeed();
void FuseRangeFinder();
void FuseOptFlow();
void GroundEKF();
void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last);
void quatNorm(float (&quatOut)[4], const float quatIn[4]);
// store staes along with system time stamp in msces
void StoreStates(uint64_t timestamp_ms);
/**
* Recall the state vector.
*
* Recalls the vector stored at closest time to the one specified by msec
*
* @return zero on success, integer indicating the number of invalid states on failure.
* Does only copy valid states, if the statesForFusion vector was initialized
* correctly by the caller, the result can be safely used, but is a mixture
* time-wise where valid states were updated and invalid remained at the old
* value.
*/
int RecallStates(float *statesForFusion, uint64_t msec);
void ResetStoredStates();
void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]);
void calcEarthRateNED(Vector3f &omega, float latitude);
static void eul2quat(float (&quat)[4], const float (&eul)[3]);
static void quat2eul(float (&eul)[3], const float (&quat)[4]);
static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD);
void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef);
static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef);
static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]);
static float sq(float valIn);
static float maxf(float valIn1, float valIn2);
static float min(float valIn1, float valIn2);
void OnGroundCheck();
void CovarianceInit();
void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
float ConstrainFloat(float val, float min, float max);
void ConstrainVariances();
void ConstrainStates();
void ForceSymmetry();
int CheckAndBound(struct ekf_status_report *last_error);
void ResetPosition();
void ResetVelocity();
void ZeroVariables();
void GetFilterState(struct ekf_status_report *state);
void GetLastErrorState(struct ekf_status_report *last_error);
bool StatesNaN();
void InitializeDynamic(float (&initvelNED)[3], float declination);
protected:
void updateDtVelPosFilt(float dt);
bool FilterHealthy();
bool GyroOffsetsDiverged();
bool VelNEDDiverged();
void ResetHeight(void);
void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
};
uint32_t millis();
uint64_t getMicros();
|