ekf2: Add support for use of multiple GPS receivers #9765

priseborough · 2018-06-26T09:33:56Z

Submitted in response to #9431

This PR gives ekf2 the ability to use two GPS receivers if available to create a blended solution where the blending weights and position/height offset correction adapts to the reported accuracy. If one receiver loses lock or data, it will automatically switch across to the good receiver.

Because a long term position offset is calculated separately for each receiver, switching between either GPS1, GPS2 and the blended solution can be performed without large position and altitude jumps.

This feature relies on use of the reported accuracy metrics contained in the vehicle_gps_position.eph, epv and s_variance_m_s messages. The EKF2_GPS_MASK parameter controls which accuracy metrics are used to set the blending ratios. By default it is zero which disables the blending. If different model receivers are used, they must both have the same update rate and both output the accuracy metrics selected by the EKF2_GPS_MASK parameter. An RTK and non RTK receiver can be used.

The EKF2_GPS_TAU parameter controls the time constant used to adjust the position offset that is applied to each GPS solution.

The blended GPS solution is published as a third GPS instance and can be found in the log as vehicle_gps_position_2

Bench test log here: https://logs.px4.io/plot_app?log=1bfcfab1-1652-48e3-b349-a42cd493e3cd

Method used to log blended GPS to a third instance may not be correct. I am relying on the uORB orb_publish_auto function to correctly assign a new instance.

Noise on blended lat/lon in log needs investigating. It was not present on logs generated using earlier replay data.

RomanBapst

@priseborough I'm 80% done with with review but need to run to another project now. Will finish this after work.

RomanBapst · 2018-06-26T12:05:46Z

src/modules/ekf2/ekf2_main.cpp

+				_gps_select_index = 1;
+
+			} else if (_gps_select_index == 2) {
+				// use last recever we received data from


RomanBapst · 2018-06-26T12:06:05Z

src/modules/ekf2/ekf2_main.cpp

@@ -572,6 +628,13 @@ void Ekf2::run()
 {
 	bool imu_bias_reset_request = false;

+	// becasue we can have a second GPS used as a truth reference to asisst with tuning and algorithm development using replay


RomanBapst · 2018-06-26T12:26:49Z

src/modules/ekf2/ekf2_main.cpp

+
+		for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+			if (_gps_state[i].fix_type >= 3 && _gps_state[i].sacc >= 0.001f) {
+				spd_blend_weights[i] = speed_accuracy_sum_sq / (_gps_state[i].sacc * _gps_state[i].sacc);


@priseborough Just for my understanding. You could also write
spd_blend_weights[i] = 1.0f / (_gps_state[i].sacc * _gps_state[i].sacc);
correct? It should not matter because below you are normalizing the weights again.

yes that is correct

RomanBapst · 2018-06-26T12:39:00Z

src/modules/ekf2/ekf2_main.cpp

+		// report the best valid accuracies and DOP metrics
+
+		if (_gps_state[i].eph > 0.0f
+		    && _gps_state[i].eph < _gps_state[GPS_BLENDED_INSTANCE].eph) {


@priseborough Did you mean to write
_gps_state[i].eph > _gps_state[GPS_BLENDED_INSTANCE].eph ?

We need to take the smallest non zero value so that won't work either. I need to initialise _gps_state[GPS_BLENDED_INSTANCE].eph to the largest float.

RomanBapst · 2018-06-26T12:40:04Z

src/modules/ekf2/ekf2_main.cpp

+		}
+
+		if (_gps_state[i].epv > 0.0f
+		    && _gps_state[i].epv < _gps_state[GPS_BLENDED_INSTANCE].epv) {


@priseborough Also here and all the other cases below.

dagar · 2018-06-27T05:57:07Z

src/modules/ekf2/ekf2_main.cpp

@@ -572,6 +628,13 @@ void Ekf2::run()
 {
 	bool imu_bias_reset_request = false;

+	// support for dual GPS inputs if available
+	int gps_subs[2];


delete? _gps_subs in class

I don't understand

You have both gps_subs[2] and _gps_subs[ORB_MULTI_MAX_INSTANCES].

I thought I needed a separate one for publishing. will fix.

dagar · 2018-06-27T05:57:27Z

src/modules/ekf2/ekf2_main.cpp

+	int gps_subs[2];
+
+	for (unsigned i = 0; i < 2; i++) {
+		gps_subs[i] = orb_subscribe_multi(ORB_ID(vehicle_gps_position), i);


Should be with _gps_subs in constructor.

dagar · 2018-06-27T05:57:59Z

src/modules/ekf2/ekf2_main.cpp

@@ -512,6 +565,8 @@ Ekf2::Ekf2():
 	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));

 	for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
+		_gps_subs[i] = -1;
+		_gps_subs[i] = orb_subscribe_multi(ORB_ID(vehicle_gps_position), i);


~~GPS_MAX_RECEIVERS for _gps_subs, not ORB_MULTI_MAX_INSTANCES.~~

dagar · 2018-06-27T05:58:12Z

src/modules/ekf2/ekf2_main.cpp

@@ -584,6 +647,12 @@ void Ekf2::run()
 	vehicle_status_s vehicle_status = {};
 	sensor_selection_s sensor_selection = {};

+	// Position of local NED origin in GPS / WGS84 frame
+	map_projection_reference_s ekf_origin = {};


I don't understand

@dagar Can you elaborate on this?

dagar · 2018-06-27T05:58:30Z

src/modules/ekf2/ekf2_main.cpp


 				ekf2_timestamps.gps_timestamp_rel = (int16_t)((int64_t)gps.timestamp / 100 - (int64_t)ekf2_timestamps.timestamp / 100);
 			}
 		}

+		// check for second GPS receiver data
+		bool gps2_updated = false;
+		orb_check(gps_subs[1], &gps2_updated);


gps_subs[1] -> _gps_subs[1]

dagar · 2018-06-27T06:02:22Z

src/modules/ekf2/ekf2_main.cpp

@@ -165,11 +198,21 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
 	const float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim;	///< preflight velocity innovation spike limit (m/sec)
 	const float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim;	///< preflight position innovation spike limit (m)

+	// GPS blending and switching
+	struct gps_message _gps_state[GPS_MAX_RECEIVERS + 1]; ///< internal state data for the physical and blended GPS


Suggestion, the code below might be a bit easier to follow if you kept the blended GPS in a separate struct rather than the end.
_gps_state[GPS_BLENDED_INSTANCE] -> _gps_blended

dagar · 2018-06-27T06:13:10Z

src/modules/ekf2/ekf2_main.cpp

@@ -165,11 +198,21 @@ class Ekf2 final : public ModuleBase<Ekf2>, public ModuleParams
 	const float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim;	///< preflight velocity innovation spike limit (m/sec)
 	const float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim;	///< preflight position innovation spike limit (m)

+	// GPS blending and switching
+	struct gps_message _gps_state[GPS_MAX_RECEIVERS + 1]; ///< internal state data for the physical and blended GPS
+	struct gps_message _gps_output[GPS_MAX_RECEIVERS + 1]; ///< output state data for the physical and blended GPS


Why is it necessary to store _gps_output? Couldn't it be discarded at the end of each update_gps_blend_states?

We can't discard it until after GPS data has been written to the EKF buffer.

I will refactor the code to remove the need for that class variable. Refactoring needs to happen anyway because there is a corner case that is not being handled correctly at the moment that results in the output not updating if one of the receivers loses lock. This was picked up in dynamic testing.

I've split the calc_gps_blend_outputs into 3 functions for updating of receiver offsets, application of receiver offsets and calculation of blended receiver output.

I'm using a class variable for outputs because I am unsure how to pass an array of structs by reference without creating a new type, and the compiler would not let me #include and use std::array to create a new type

RomanBapst · 2018-06-27T20:38:41Z

@priseborough I'm done with review (can't approve on my phone). We will test it tomorrow.

priseborough · 2018-06-28T04:50:21Z

There was a bug found during dynamic ground testing that caused the GPS output state to not be updated f one of the receivers was disconnected or lost lock. I've split the calc_gps_blend_outputs() function into three separate functions for updating steady state (filtered) offsets, correcting physical receiver data using the offsets and calculating a blended output using the corrected receiver data.

Testing on replay shows the algorithm is working properly. However, although printing the lat and lon values in the gps struct one line above where they are published shows the correct values, when I review the log generated by the ekf2 replay, the vehicle_gps_position_2.lat and lon values are not correct so I have a mistake in the publishing of the blended GPS.

priseborough · 2018-06-28T04:52:24Z

The CI is showing px4fmu v2 to be out of flash space.

priseborough · 2018-06-28T08:10:16Z

Bench testing at commit 935b701 shows logging of the third (blended) GPS instance working as expected, so the logging problem appears to a replay issue.

The remaining issue is the large initialisation error in the blended solution for lat and lon that takes several seconds to remove - see bench test result below:

Replaying the same data gives a much smaller startup transient - see

priseborough · 2018-06-28T10:19:13Z

Last commit 79f41ce seems to have resolved remaining issues. Here are GPS alt, lat and lon from the last test:

priseborough · 2018-06-28T11:46:25Z

After experimenting with some logging options with bench testing, it appears that unless estimator replay data logging is enabled via SDLOG_PROFILE, then the GPS blending does not work correctly and the blended instance logged closely tracks the second receiver . I suspect that publishing of the blended instance could be overwriting data from a physical receiver.

RomanBapst · 2018-06-28T18:39:59Z

@priseborough We did two flights with dual gps:

In the first one something went wrong, one gps position was showing showing somewhere in the pacific
https://review.px4.io/plot_app?log=0a88185a-28b7-4ce8-b048-328b961d44a6

The second try was good, we managed to fly in position control mode. Maybe it was drifting a bit more than usual but it might not be related to the changes
https://review.px4.io/plot_app?log=4d30ebe3-48f0-433b-8fb9-b1eaf6da5392

I haven't had a chance to look into them.

hamishwillee · 2018-06-28T23:49:17Z

I hope that when this support is in system through to QGC "someone" will let me know so we can update the docs.

priseborough · 2018-06-29T06:16:24Z

@hamishwillee Good reminder. We will be setting the parameters so that the feature is disabled by default. Is there a way to flag a feature in master as 'beta' until we are happy it has had enough community test coverage?

hamishwillee · 2018-06-29T07:08:57Z

Good reminder. We will be setting the parameters so that the feature is disabled by default. Is there a way to flag a feature in master as 'beta' until we are happy it has had enough community test coverage?

There is no "formal" way. What I normally do is either
a) Document after it is "ready"
b) Put a big warning near where the feature is documented.

Is there any specialized QGC UI for setup, or is this just going to be a parameter? Obviously we now have to explain what ports you can add the second GPS to, how to turn it on, any tuning, and how to turn off anything else that might be using the port.

I would probably add doc here: https://docs.px4.io/en/gps_compass/, and possibly also here: https://docs.px4.io/en/advanced_features/

priseborough · 2018-06-29T07:16:31Z

@hamish Just one parameter for most applications. There is second time constant parameter controlling how fast the individual GPS offsets are corrected, but I am hoping we can tune that so that 99% of users don't need to touch it.

priseborough · 2018-06-29T07:57:37Z

@RomanBapst The first log appears to be afflicted by the same initialisation bug affecting the lat and lon output that I was chasing and fixed yesterday. The second log shows the blending working as expected for alt, lat, lon and velocity other than 0.4m 'switching' noise on the lat/lon output that appears at certain times. Running the data through the latest version reproduces the noise so this can be investigated and fixed. It does not affect the altitude, so is not an issue with the weighting calculation and appears to be a problem in the way global coordinates are converted to local position deltas and vice versa.

Data from the the second flight:

hamishwillee · 2018-06-29T22:49:02Z

@hamish Just one parameter for most applications. There is second time constant parameter controlling how fast the individual GPS offsets are corrected, but I am hoping we can tune that so that 99% of users don't need to touch it.

Thanks. YOu could put in a Note in the param definition itself warning that the feature is still not robust, but you would have to remove it later.

I suspect users will also need to use a param to turn off whatever is using the port to which you attach the second GPS. ie SYS_COMPANION

priseborough · 2018-06-29T23:48:41Z

Writing the blended GPS to a third instance of the GPS message is causing problems on replay with the lat,lon values in vehicle_gps_position_2 not matching the values logged using a custom message because the replay and ekf2 are both publishing to that instance.

@dagar is there any way I can make ekf2 write the blended output to a specified instance (eg index 3) when doing replay?

Prevents large transients at startup.

Previous calculation attempted to use a shortcut that created a circular data dependency.

Also fix formatting.

dagar · 2018-07-22T15:49:37Z

Rebased on master and currently fitting into px4fmu-v2.

dagar · 2018-07-22T16:17:27Z

Let's get this in!

lingbuweibu · 2018-11-19T09:18:52Z

If the eph or epv or evh is not change,but the lon or lat is jump,how can I do?

lingbuweibu · 2018-11-19T09:30:08Z

// Use the oldest non-zero time, but if time difference is excessive, use newest to prevent a disconnected receiver from blocking updates
uint64_t max_us = 0; // newest non-zero system time of arrival of a GPS message
uint64_t min_us = -1; // oldest non-zero system time of arrival of a GPS message

for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
	// Find largest and smallest times
	if (_gps_state[i].time_usec > max_us) {
		max_us = _gps_state[i].time_usec;
	}

	if ((_gps_state[i].time_usec < min_us) && (_gps_state[i].time_usec > 0)) {
		min_us = _gps_state[i].time_usec;
	}
}

The min_us in here is a bug？？？

dagar · 2018-11-19T15:44:40Z

@lingbuweibu no, that's initializes min_us to the largest uint64 value so that the comparison sets min_us to an actual gps time.

lingbuweibu · 2018-11-20T00:17:29Z

@lingbuweibu no, that's initializes min_us to the largest uint64 value so that the comparison sets min_us to an actual gps time.

OK

lingbuweibu · 2018-11-23T08:28:03Z

Today, I did an experiment，the first was RTK,the fix was six,the second gps was M8N,the fix was four,the "_gps_select_index" was always = "2",when I plug the RTK gps,the data of RTK gps was not changed,the fix was always six, the "_gps_select_index" was always = "0",what can I do to solve the problem?

priseborough added the Admin: Enhancement (improvement) 💡 label Jun 26, 2018

priseborough self-assigned this Jun 26, 2018

priseborough requested a review from RomanBapst June 26, 2018 09:33

priseborough added EKF2 proposal labels Jun 26, 2018

priseborough requested a review from dagar June 26, 2018 10:02

RomanBapst requested changes Jun 26, 2018

View reviewed changes

dagar reviewed Jun 27, 2018

View reviewed changes

priseborough force-pushed the pr-ekfDualGps branch 2 times, most recently from 027b2b1 to 115711a Compare June 28, 2018 02:12

priseborough and others added 17 commits July 22, 2018 11:48

ekf2: limit GPS offset states

c7bf2e6

Prevents large transients at startup.

ekf2: remove unnecessary braces

e14ff7a

ekf2: fix bug in calculation of output blended lat lon

acd1010

Previous calculation attempted to use a shortcut that created a circular data dependency.

ekf2: Fix blending of offset corrected GPS data

0b0835b

ekf2: gps blending - remove unnecessary calculations

b0458c1

msg: Add topic to log ekf2 GPS selection

1f42a4b

ekf2: log to separate topic

864bd25

ekf2: If user enables GPS blending, always log selected or blended data.

47487e5

ekf2: fix post rebase build errors

1fbf5ea

ekf2: Fix bug preventing GPS use when blending disabled

9ea1aed

Also fix formatting.

ekf2: Fix CI build error

38692a7

msg list sort

9f20241

ekf2 sort headers

b92f30d

ekf2 remove unnecessary _gps_subs init

5ceb853

ekf2 initiailize all class members

807a14b

ekf2 remove duplicate setGpsData and combine logic

9ff3e96

ekf2 ekf_gps_position publish remove unnecessary param check

49a361b

dagar force-pushed the pr-ekfDualGps branch from 7d4ab4f to 49a361b Compare July 22, 2018 15:49

dagar approved these changes Jul 22, 2018

View reviewed changes

dagar merged commit fc65939 into PX4:master Jul 22, 2018

priseborough deleted the pr-ekfDualGps branch July 23, 2018 05:19

Antiheavy mentioned this pull request Jul 31, 2018

WIP: mavlink GPS_INPUT support #10093

Closed

KeyneZzz mentioned this pull request Aug 15, 2018

problem with dual gps, or GPS2 port #10233

Closed

ekf2: Add support for use of multiple GPS receivers #9765

ekf2: Add support for use of multiple GPS receivers #9765

Conversation

priseborough commented Jun 26, 2018 • edited by AuterionWrikeBot Loading

RomanBapst left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dagar Jun 27, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

RomanBapst commented Jun 27, 2018

priseborough commented Jun 28, 2018

priseborough commented Jun 28, 2018

priseborough commented Jun 28, 2018

priseborough commented Jun 28, 2018

priseborough commented Jun 28, 2018

RomanBapst commented Jun 28, 2018

hamishwillee commented Jun 28, 2018

priseborough commented Jun 29, 2018

hamishwillee commented Jun 29, 2018

priseborough commented Jun 29, 2018

priseborough commented Jun 29, 2018

hamishwillee commented Jun 29, 2018

priseborough commented Jun 29, 2018

dagar commented Jul 22, 2018

dagar commented Jul 22, 2018

lingbuweibu commented Nov 19, 2018

lingbuweibu commented Nov 19, 2018

dagar commented Nov 19, 2018

lingbuweibu commented Nov 20, 2018

lingbuweibu commented Nov 23, 2018

priseborough commented Jun 26, 2018 •

edited by AuterionWrikeBot

Loading

dagar Jun 27, 2018 •

edited

Loading