LPE vs EKF2 position control with only GPS

deksprime · April 4, 2017, 9:43pm

Hello all

This is a position control effort with EKF2 with current stable
http://logs.uaventure.com/view/PM6byrEZRGC6pn5FLeVm8X

This is a position control effort with LPE with current stable
http://logs.uaventure.com/view/xkjq7T6JJVPzobXTBNsRu8

Has anyone got an idea why local position x and y differ this much? I haven’t seen a weird behaviour like this on local position x and y before with LPE.

dagar · April 9, 2017, 7:17pm

@Paul_Riseborough @jgoppert @LorenzMeier any thoughts here?

jgoppert · April 9, 2017, 8:58pm

The local coordinate reference latitude/longitude is incorrectly set to Zurich. This is set as default for flights where GPS is not used. Somehow this logic was reached:

github.com

PX4/Firmware/blob/00334ad76d03b5abc2144f1ebba7faf691448f5c/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp#L377


	}


	if (_validTZ) {
		_time_last_tz = _timeStamp;
	}


	// check timeouts
	checkTimeouts();


	// if we have no lat, lon initialize projection at 0,0
	if (_validXY && !_map_ref.init_done) {
		map_projection_init(&_map_ref,
				    _init_origin_lat.get(),
				    _init_origin_lon.get());
	}


	// reinitialize x if necessary
	bool reinit_x = false;


	for (int i = 0; i < n_x; i++) {
		// should we do a reinit

So somehow you got a valid xy positoin without the map ref being initialized. GPS intializes the map ref here:

github.com

PX4/Firmware/blob/00334ad76d03b5abc2144f1ebba7faf691448f5c/src/modules/local_position_estimator/sensors/gps.cpp#L65


			_gpsAltOrigin = gpsAlt + _x(X_z);


			// find lat, lon of current origin by subtracting x and y
			double gpsLatOrigin = 0;
			double gpsLonOrigin = 0;
			// reproject at current coordinates
			map_projection_init(&_map_ref, gpsLat, gpsLon);
			// find origin
			map_projection_reproject(&_map_ref, -_x(X_x), -_x(X_y), &gpsLatOrigin, &gpsLonOrigin);
			// reinit origin
			map_projection_init(&_map_ref, gpsLatOrigin, gpsLonOrigin);


			// always override alt origin on first GPS to fix
			// possible baro offset in global altitude at init
			_altOrigin = _gpsAltOrigin;
			_altOriginInitialized = true;


			PX4_INFO("[lpe] gps init "
				 "lat %6.2f lon %6.2f alt %5.1f m",
				 gpsLatOrigin,
				 gpsLonOrigin,

This is what it takes for validXY to be true:

github.com

PX4/Firmware/blob/00334ad76d03b5abc2144f1ebba7faf691448f5c/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp#L317


		updateSSParams();
	}


	// is xy valid?
	bool vxy_stddev_ok = false;


	if (math::max(_P(X_vx, X_vx), _P(X_vy, X_vy)) < _vxy_pub_thresh.get()*_vxy_pub_thresh.get()) {
		vxy_stddev_ok = true;
	}


	if (_validXY) {
		// if valid and gps has timed out, set to not valid
		if (!vxy_stddev_ok && !_gpsInitialized) {
			_validXY = false;
		}


	} else {
		if (vxy_stddev_ok) {
			if (_flowInitialized || _gpsInitialized || _visionInitialized || _mocapInitialized) {
				_validXY = true;
			}

So you must have initialized flow, vision, or mocap before GPS which if you are going to do that you need to set LPE_LAT, LPE_LON to the correct values before takeoff.

deksprime · April 10, 2017, 9:24am

@jgoppert I didn’t actually use anything but GPS on these tests, but there is a strange behaviour when I use LPE, the map starts at Zurich and very rapidly converges to my current position. Sometimes it starts at Zurich and doesn’t converge, but the copter acts as if it was really in Zurich and when I fly it thinks that it flies around eth zurich . Its really strange and I couldn’t debug. The exact same thing you’ve described is happening.

jgoppert · April 10, 2017, 1:08pm

There must be some bug that we didn’t catch in the release that you are running into as a result of your particular setup. We recently added LPE_FAKE_ORIGIN (1 enable fake initialization at LPE_LAT, LPE_LON, 0 disable it). So whatever is happening with your system, this would probably resolve it.