strip out dependencies on PX4 Firmware, create hooks for other dev environments to connect/use this library, e.g. ros.

The heading fusion observation noise parameter EKF2_HEAD_NOISE adjusts the observation variance, not the observation noise. It should be adjusting noise and have units of rad
The magnetometer noise parameter EKF2_MAG_NOISE ias actually adjusting the magnetic field state process noise, nor the magnetometer observation noise. It should be renamed.
There is no parameter to adjust the magnetometer observation noise
There is no parameter to control which magnetometer fusion method is used. A suggestion is to provide three options

1. Auto selection (default)
2. Always used heading fusion
3. Always use 3-axis field fusion

We need to add a separate filter monitoring function that looks at innovations, state variances, fusion timeout, GPS quality metrics, etc to:
a) Provide a position accuracy estimate to the system
b) Provide a dead-reckoning status to the system
c) Provide a validity status for the different states (eg attitude, hvel, hpos, vvel, vpos, wind, etc)
d) Provide position glitch warning to the system

The following replay log produces a NaN during alignment

http://logs.uaventure.com/view/9gSdQdUgMevQUdj6gZ44aL

@Tumbili
From two flights with the wingwing

The raw airspeed sensor data doesn't look so nice so will try to replace the airspeed sensor and level shifter with a new one from AUAV and try again.

Ecl and PX4/Firmware/src/modules/controllib have the same design and same purpose and should be combined. Basically all control blocks look like:

y = update(u)

With matrix lib we can even make all of these blocks in vector versions.
The controllib is set up similar to simulink. Each block has a set of parameters which form its context, and these blocks can be nested. When you call you call y = update(u), then the super block, can call update on all required subblocks to produce the output.

The parameter backend of the contorllib is currently tied to px4 internals, but this would be very easy to switch by modifying the controllib/blocks/Block.hpp

@Tumbili @LorenzMeier @priseborough

When using external vision, external vision height is never used for height update as the baro flag does not go "off" and the if(baro) else if(external vision) fails to use external vision height. Need to reset baro flag when using external vision correctly.

For now, changing

if (_fuse_height) {
    if (_control_status.flags.baro_hgt) {
        fuse_map[5] = true;
        // vertical position innovation - baro measurement has opposite sign to earth z axis
        _vel_pos_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
        // observation variance - user parameter defined
        R[5] = fmaxf(_params.baro_noise, 0.01f);
        R[5] = R[5] * R[5];
        // innovation gate size
        gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);

    } else if (_control_status.flags.gps_hgt) {
        fuse_map[5] = true;
        // vertical position innovation - gps measurement has opposite sign to earth z axis
        _vel_pos_innov[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
        // observation variance - receiver defined and parameter limited
        // use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
        float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
        float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
        R[5] = 1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit);
        R[5] = R[5] * R[5];
        // innovation gate size
        gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);

    } else if (_control_status.flags.rng_hgt && (_R_to_earth(2, 2) > 0.7071f)) {
        fuse_map[5] = true;
        // use range finder with tilt correction
        _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_to_earth(2, 2),
                             _params.rng_gnd_clearance));
        // observation variance - user parameter defined
        R[5] = fmaxf(_params.range_noise, 0.01f);
        R[5] = R[5] * R[5];
        // innovation gate size
        gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
    }

}

to

if (_fuse_height) {
    if (_control_status.flags.baro_hgt && !_control_status.flags.gps && !_control_status.flags.ev_pos && !(_control_status.flags.rng_hgt && (_R_to_earth(2, 2) > 0.7071f))) {
        fuse_map[5] = true;
        // vertical position innovation - baro measurement has opposite sign to earth z axis
        _vel_pos_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset;
        // observation variance - user parameter defined
        R[5] = fmaxf(_params.baro_noise, 0.01f);
        R[5] = R[5] * R[5];
        // innovation gate size
        gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);

    } else if (_control_status.flags.gps_hgt) {
        fuse_map[5] = true;
        // vertical position innovation - gps measurement has opposite sign to earth z axis
        _vel_pos_innov[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
        // observation variance - receiver defined and parameter limited
        // use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
        float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
        float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
        R[5] = 1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit);
        R[5] = R[5] * R[5];
        // innovation gate size
        gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);

    } else if (_control_status.flags.rng_hgt && (_R_to_earth(2, 2) > 0.7071f)) {
        fuse_map[5] = true;
        // use range finder with tilt correction
        _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_to_earth(2, 2),
                             _params.rng_gnd_clearance));
        // observation variance - user parameter defined
        R[5] = fmaxf(_params.range_noise, 0.01f);
        R[5] = R[5] * R[5];
        // innovation gate size
        gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f);
    } else if (_control_status.flags.ev_pos) {
        // warnx("fusing ext_visn_hight");
        fuse_map[5] = true;
        // calculate the innovation assuming the external vision observaton is in local NED frame
        _vel_pos_innov[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
        // observation variance - defined externally
        R[5] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
        R[5] = R[5] * R[5];
        // innovation gate size
        gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f);
    }

}

enables use of external vision height.

Did two flights with the wingwing

First flight
Plots: http://logs.uaventure.com/view/8gD2HCcvqjobKm5Bq46Yei
Log: http://logs.uaventure.com/logmuncher/charts/8gD2HCcvqjobKm5Bq46Yei/logmuncher.log

Second flight
Plots: http://logs.uaventure.com/view/hbXUjHrcGHQm9Dpuxxcv5d
Log: http://logs.uaventure.com/logmuncher/charts/hbXUjHrcGHQm9Dpuxxcv5d/logmuncher.log

@priseborough @Tumbili
Would be very happy for any comments on the logs

For fixed wing applications, a aligning the initial heading using the GPS velocity and reinitialising the magnetic field states shortly after takeoff or launch is required to enable operation without a magnetometer or to recover from a bad magnetometer setup. An example of this functionality can be found in the APM EKF2 alignYawGPS() function.

Enables planes to estimate wind without an airspeed sensor and improves dead reckoning performance when GPS is lost

The simpler magnetic heading fusion should be used when on the ground or indoors, with a transition to use of 3-axis magnetometer fusion for up and away flight. This is because the simple magnetic heading fusion is more robust to external field disturbances, whereas the 3-axis fusion is more robust to sensor errors and provides improved angular accuracy when the assumption of a consistent external field is true.

When the GPS is connected to the pixhawk and the GPS module is started, but external vision is enabled for position and altitude instead of GPS, the reference altitude of local position somehow also drifts with the GPS value(indoors). Turning off the GPS module or disconnecting it solves the issue.

I suspect its the estimators "_gps_alt_ref" value is leaking in from GPS somehow.

Yesterday we did a flight test with a fixed wing and ekf2. I adopted the current master code to always fuse airspeed above 3 meters per second just to verify if the calculations are correct. @priseborough @Tumbili It would be great if you could have a look and tell me what you think of the innovations?

Flight outside with Wingwing, fusing airspeed at 12.5 Hz

Same code in SITL, fusing airspeed at 12.5 Hz

Same code in SITL, fusing airspeed at 1 Hz

At initialization LPOS.X and LPOS.Y take way too long to converge to values sent by external vision. (Probably because aircraft is stationary but not at origin when EKF2 starts, so external vision is sending in non zero values). Need to support starting at non zero values.

And they oscillate on external vision reset to new value:

External vision data is fused with 1cm variance.

Use of the in-built sensor digital noise filtering increases effect of coning errors and does not reduce effect of aliasing. The use of the IMU inbuilt digital filters should be minimised.
Coning error corrections should be applied before the gyro data is integrated to form delta angles and down-sampled to 400Hz.
Noise filtering for the angular rate and specific force measurements used by control loops should be done before the data is down-sampled to 400Hz using a Butterworth or equivalent IIR minimum group delay filter.

@priseborough @Tumbili
I am working on the logic of when to fuse syntetic sideslip measurement. The user of the ecl library can control when to fuse airspeed by controlling when to push airspeed measurements into the ekf. This is not possible for sideslip, so do we need to create a flag that is set outside the library to control this?
Or should we implement something that works for most users, e.g. only fuse sideslip as a fallback when we lose airspeed measurements and then people will have to hack the library if they want to do something else, e.g. for vtol platforms?
What do you think?

The EKF has to learn the declination starting from an initial value of zero for each flight. We should calculate an initial value using the WGS-84 position or remember the value the end of each flight.

The parameter instances are never deleted in the constructor.
This burns 10kB of memory :-)

This is a placeholder to capture miscellaneous ideas for improvement:

Efficiency improvements can be found by limiting the maximum index during covariance and state operations depending on whether magnetic field learning is required and whether wind state estimation is required. Doing so reduces the effective number of filter states to 16 for most multi-rotor applications.

Initial angular alignment should be done in stages with no yaw alignment and magnetometer fusion until the tilt error as determined by the filtered x,y length of the angle error state vector has decayed.

Coning corrections need to be added to the inertial sensor library. EKF2 assumes that these have already been applied to the delta angles.

A default filter tune that that prioritises robustness over accuracy has been implemented. It should be possible to have a second tune available that provides increased accuracy at the expense of robustness for experienced user setups.

Happened on a test flight with wing wing and Pixracer

http://logs.uaventure.com/view/EDXp7BmVQsSsboydZMKVWW

First insights from discussion with @priseborough:

• when local position state is reset the corresponding velocity states needs to be reset as well (not the case now)
• the high innovations and the resulting divergence of the vertical position could have been caused by timing issues (need to check the performance results)

I'm trying to build this following the instructions of the README.md.

~/src/Firmware/src/lib/ecl/build
[127] % make
[  8%] Building CXX object CMakeFiles/ecl.dir/estimator_interface.cpp.o
In file included from /home/julianoes/src/Firmware/src/lib/ecl/EKF/geo.h:49:0,
                 from /home/julianoes/src/Firmware/src/lib/ecl/EKF/estimator_interface.h:45,
                 from /home/julianoes/src/Firmware/src/lib/ecl/EKF/estimator_interface.cpp:46:
/home/julianoes/src/Firmware/src/lib/ecl/EKF/mathlib.h:44:23: fatal error: Eigen/Dense: No such file or directory
compilation terminated.
CMakeFiles/ecl.dir/build.make:62: recipe for target 'CMakeFiles/ecl.dir/estimator_interface.cpp.o' failed
make[2]: *** [CMakeFiles/ecl.dir/estimator_interface.cpp.o] Error 1
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/ecl.dir/all' failed
make[1]: *** [CMakeFiles/ecl.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
make: *** [all] Error 2

This is on Ubuntu 16.04.

I am trying to get the snapdragon flying in altitude control based on altitude estimation with the TeraRanger One. However, local_position_NED.z is very inaccurate and seems to depend a lot on the quality of the acceleration measurements.

I'm testing this the following way: I've hardcoded the TeraRanger driver to output a distance of 0.3m. EKF2_RNG_NOISE is set to 0.0001. The quad sits stationary on the ground. After a few seconds, I arm, after a few more seconds I increase the throttle.

While disarmed, local_position_NED.z already oscillates too much for my taste given the low measurement uncertainty. After arming, oscillation increases a lot (+/-10cm). See this log: http://logs.uaventure.com/view/5SfAq53SPzWRjmnxWUHF3U#Local_Z_PLOT

This oscillation appears largely independent of EKF2_RNG_NOISE and EKF2_ACC_NOISE. The problem seems to be the complementary filter. EST0.s8 tracks the range measurements quite reasonably, it's just LPOS.Z that oscillates.
I've confirmed this by changing this line to:

_output_new.pos += _vel_corr * imu_new.delta_vel_dt;

Now the estimated altitude looks much better: http://logs.uaventure.com/view/635VkhfaZqYAmDcM5ZeuDX#Local_Z_PLOT

Thus, the problem is the noise in delta_vel_NED(2), which cannot be tuned away by any EKF2 parameters, as far as I can see.

What is the suggested action to improve the altitude estimate?

@priseborough This is to track the baro offset estimation.

The in-air/on-ground logic currently uses system arm status and height change. Existing landing check information should also be used.

After following the advice given in #111, we have come to the conclusion that altitude estimation performance of the complementary filter is still not satisfactory.

This can be seen in the following image:

EST0.s8 tracks the range data (the sign of which is inverted in the shown plot) well, while LPOS.Z does not. LPOS.Z often overshoots in tracking changes in altitude. At 55s, LPOS.Z does something completely different than EST0.s8. Between 20s and 40s, the quad is placed on the ground (the range sensor saturates at its minimum value) and is disarmed (thus, IMU values are not noisy). Still, LPOS.Z overshoots significantly and appears to saturate at an offset value.

The depicted log is here: http://logs.uaventure.com/view/CYBGUXRipknC9Rr5Ug9kN4

Here is another replay log of my current setup: http://logs.uaventure.com/view/KphLtTVUAttaQhPtUf5XBn

The firmware is on ffb0d37 with all submodules updated.

On top of that I have added a low pass filter for the acceleration measurements fed into the control_status with a cutoff frequency of 30Hz, as suggested in the above-mentioned issue. I have also added the control_status acc measurements to the log file. However, it appears as though we have an estimation problem and not (or not only) a control problem.

@priseborough can you help us with this?

If GPS is lost, then a maximum dead-reckoning time must be observed, at which time the filter will need to be switched into its non-aiding mode of operation to prevent loss of attitude reference.

If the innovation consistency check on GPs position has failed for long enough that the horizontal position uncertainty has reached a user specified size, then the states should be reset to the GPS position. This is to stop bad inertial data causing permanent loss of GPs fusion which can happen if the inertial solution errors are growing faster than the innovation consistency gate.

A significant reduction in output predictor tracking errors due to bad IMU data has been achieved here:

ArduPilot/ardupilot#4399

This output observer concept needs to be implemented in ecl

I'm interested in using EKF2 for fixed wing flights, but I see airspeed fusion is currently disabled. What needs to be done to enable airspeed fusion? I can help with testing and debugging.

https://github.com/PX4/ecl/blob/eaae95fdc4e48e3946a463941dfb9d5e495e975f/EKF/control.cpp#L744

The initialization in SITL is painfully slow and consumes a lot of time for developers.

The two delays to wait for are:

• ekf2 initialized
• home position initialized

Presumably both are due to the rather slow startup of ekf2.

It would be nicer to have this faster, or at least use a hack to speed it up for SITL.

This user log with a pixhack board had large vibration errors on the accelerometer and very noisy baro data that caused a temporary loss of height reference. A different set of tuning defaults would have prevented this.

Note: The internal isolation design of the pixhack is poor and can cause clipping if resonance of the mount occurs. Unfortunately there are lots of those boards out there and users do not think they need to be isolated.

http://logs.uaventure.com/view/DJwKnqsFtHVJQuaKoEK8UY

This particular log would likely require a increase in baro observation noise to 3.0m and accel process noise to 0.5 m/s/s which would come at a cost of reduced angular accuracy and increased susceptibility to GPS glitches.

The delta angle and delta velocity auto-code variables in the covariance prediction calculations are supposed to be using uncorrected values, eg

ArduPilot/ardupilot@501845e

Per control.cpp#L452-L470, what is the motivation for adding the _control_status.flags.XX as logical ors inside the if/else statements for baro/gps? Are we trying to limit the height sensor fusion to one sensor only or fuse all that are available?

If the prior, seems to make sense to add add a param check for VDIST_SENSOR_EV (which exists in the enum already) to the first if statement and remove the _control_status.flags checks for baro/gps. If the later is the case, what purpose does the _params.vdist_sensor_type serve?

When operating with optical flow or some other relative aiding method, the absolute headin.g and magnetic declination is unobservable. To enable use of the more accurate 3-axis magnetometer fusion, fusion of synthetic declination observations is required to constrain declination drift and loss of absolute heading reference. The method to be implemented is here:

https://github.com/PX4/ecl/blob/master/matlab/generated/Inertial%20Nav%20EKF/Magnetic%20Declination%20Fusion.txt

Currently being implemented on this branch: https://github.com/priseborough/ecl/tree/pr-addMagErrHandling

Ran some more motion tests today using the EV fusion updates that were pulled into master a few days ago (and the new alignment PR from yesterday -- alignment seems much faster). I'm passing in vision updates at 100Hz and they are pretty clean. There's quite of a bit of overshoot and settling time in the ekf (as compared to the vision measurements) that I'm struggling to tune out. Eg., as I'm walking with the drone, when I physically stop motion, the vision measurements flatline, but the ekf oscilates twice or so and eventually settles. I've played around with the noise parameters for both EV and accel/accel bias, with no clear trends to reduce the overshoot. Not sure what other nobs I can turn. Could use some advice. Log file with replay enabled here (not sure why the LPOS data isn't show up on logmuncher?): http://logs.uaventure.com/view/SWMESe69SkMENaTSPwHwcZ

Thanks!

@LorenzMeier @priseborough
Dual GPS is now supported in PX4/Firmware but not in EKF2. Here is a plot of a switch of module.

We should specify how to handle it and we would be happy to work on it.

• Increase the airspeed readout rate to 14 Hz and
• And test in SITL
• Make the estimator publish to the wind topic PX4/PX4-Autopilot#4147
• Optimize the calculation of the covariance matrix by taking advantage of the sparseness in H
• Add protection against the denominator in line 76 being too small #86
• Move the check for if we want to fuse airspeed somewhere higher up
• Implement the logic of when to fuse airspeed, i.e. not on the ground etc.
• Make sure the wind states and covariances are initialized correctly when we start fusing airspeed
• Clean up the use of TAS/EAS
• Fix the issue with landed detected PX4/PX4-Autopilot#4155
• Rename _airspeed_sample_delayed.airspeed to _airspeed_sample_delayed.true_airspeed

After switching from baro to gps we initialize the primary height sensor offset (in this case gps) like this
https://github.com/PX4/ecl/blob/master/EKF/control.cpp#L186

However, we should not be using the current height state value in this calculation.
@priseborough Please have a look.

@priseborough For urban scenarios it would be beneficial to just have drift but not divergence, in particular in the presence of excellent VIO based heading estimates.

@priseborough @ndepal @CarlOlsson @boosfelm This is to discuss the effects of medium to strong vibration we saw on EKF2 and the relationship to other estimators which seemed to survive these better or at least hide the problem in a way that we felt they were fine to operate.

Could you share recent replay logs of these and maybe also a baseline flight with EKF1?

This is related to #125

Addition of a separate filter monitoring function that looks at GPS innovations, eph, epv, speed accuracy, number of satellites, position variances, etc to determine if
a) Is the GPS solution is good enough to set an origin
b) Provide a position accuracy estimate to the system
c) Provide a dead-reckoning status to the system
d) Provide a validity status for the different states (eg attitude, hvel, hpos, vvel, vpos, wind, etc)
e) Provide position glitch warning to the system

Hi guys,
I'm doing some fusion algorithm research and has some work based on your code, so could you make the repo more friendly to be cited? The instruction is here.
Many thanks!

This has contributed to a crash caused by large magnetometer errors when the vehicle switched from yaw to 3D fusion.

Should be done in stages with no yaw alignment and magnetometer fusion until the tilt error as determined by the filtered x,y length of the angle error state vector has decayed.

A lot of handwritten interface code is being done in ecl, for instance, GPS message structures etc. I think we should autogenerate these interfaces. Why not even using the existing uorb message generation in PX4/Firmware. This will keep ECL platform independent and make it easier to develop/read/maintain.

@Tumbili @LorenzMeier @priseborough