px4 / px4-ecl Goto Github PK
View Code? Open in Web Editor NEWEstimation & Control Library for Guidance, Navigation and Control Applications
Home Page: http://dev.px4.io
License: BSD 3-Clause "New" or "Revised" License
Estimation & Control Library for Guidance, Navigation and Control Applications
Home Page: http://dev.px4.io
License: BSD 3-Clause "New" or "Revised" License
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
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!
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.
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.
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.
@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.
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.
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
A significant reduction in output predictor tracking errors due to bad IMU data has been achieved here:
This output observer concept needs to be implemented in ecl
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:
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
This has contributed to a crash caused by large magnetometer errors when the vehicle switched from yaw to 3D fusion.
Enables planes to estimate wind without an airspeed sensor and improves dead reckoning performance when GPS is lost
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
Currently being implemented on this branch: https://github.com/priseborough/ecl/tree/pr-addMagErrHandling
@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
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.
@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
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!
strip out dependencies on PX4 Firmware, create hooks for other dev environments to connect/use this library, e.g. ros.
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
The following replay log produces a NaN during alignment
The parameter instances are never deleted in the constructor.
This burns 10kB of memory :-)
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?
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.
@priseborough This is to track the baro offset estimation.
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.
@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?
Happened on a test flight with wing wing and Pixracer
http://logs.uaventure.com/view/EDXp7BmVQsSsboydZMKVWW
First insights from discussion with @priseborough:
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.
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.
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.
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.
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?
The delta angle and delta velocity auto-code variables in the covariance prediction calculations are supposed to be using uncorrected values, eg
The in-air/on-ground logic currently uses system arm status and height change. Existing landing check information should also be used.
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.
The initialization in SITL is painfully slow and consumes a lot of time for developers.
The two delays to wait for are:
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.
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.
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?
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.
@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.
A declarative, efficient, and flexible JavaScript library for building user interfaces.
๐ Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. ๐๐๐
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google โค๏ธ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.