-
Notifications
You must be signed in to change notification settings - Fork 12
/
Usage.txt
43 lines (33 loc) · 2.43 KB
/
Usage.txt
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
Assumes MEAS MS5611 barometeric pressure sensor, Maxim MAX21100 gyro+accel, Madgwick AHRS algorithm
// Z_VARIANCE and ZACCEL_VARIANCE can be configured to shape the response of the filter to your specific application and personal
// preferences
// Z_VARIANCE is the measured sensor altitude cm noise variance, with sensor at rest, normal sampling rate, and 1-2 seconds max samples
// I personally use a somewhat smaller value than the measured variance as I favour a faster response to step inputs and am willing to
// tolerate a bit of jitter with the unit at rest.
#define Z_VARIANCE 300.0f
// The filter models acceleration as an external environmental disturbance to the system, ZACCEL_VARIANCE is the estimated
// variance of the perturbations. For a paragliding application, this would be the expected acceleration variance due to thermal
// activity, how sharp the thermal edges are, etc. This is NOT the accelerometer sensor noise variance. Increase this value and the
// filter will respond faster to acceleration inputs.
#define ZACCEL_VARIANCE 200.0f
// The accelerometer bias (offset between true acceleration and measured value) is not likely to change rapidly, so a low value
// of ZACCELBIAS_VARIANCE will enforce that. But too low a value, and the filter will take longer on reset to settle to the estimated
// value. For an audio variometer, the symptom would be the variometer beeping for several seconds after reset, with the unit at rest.
#define ZACCELBIAS_VARIANCE 1.0f
initialization
...
ms5611.AveragedSample(4); // pressure sensor based estimate of altitude in cm for initializing the kalman filter.
// Use initial velocity = 0. Unit would have to be at rest anyway for gyroscope bias calibration.
kalman.Configure(Z_VARIANCE, ZACCEL_VARIANCE, ZACCELBIAS_VARIANCE, ms5611.zCmAvg_,0.0f,0.0f);
...
loop
...
ms5611.SampleStateMachine();
max21100.GetAccelValues(ax,ay,az,&fax, &fay, &faz);
float fa = sqrt(fax*fax + fay*fay + faz*faz);
// Use only gyroscope readings in orientation estimation, if accel magnitude is very different from 1G
int bUseAccel = ((fa > 0.6f) && (fa < 1.4f)) ? 1 : 0;
imu_MadgwickQuaternionUpdate(bUseAccel,elapsedTimeSecs,fax,fay,faz,fgx*PI_DIV_180,fgy*PI_DIV_180,fgz*PI_DIV_180,fmx,fmy,fmz);
float accel = 980.0f*imu_GravityCompensatedAccel(fax,fay,faz,quat);
kalman.Update((float)ms5611.zCmSample_, accel, elapsedTimeSecs, &zTrack, &vTrack);
...