Position Hold with VISION_POSITION_ESTIMATE and ATT_POS_MOCAP for indoor localization

ArduCopter
, , ,
28

Hi @Subodh_Mishra,
When I fly my drone in an Indoor environment based on VIVE HTC, I override the following functoins:

  1. bool NavEKF3_core::getPosNE(Vector2f &posNE) const
  2. bool NavEKF3_core::getPosD(float &posD) const
  3. bool NavEKF3_core::getHAGL(float &HAGL) const
  4. bool NavEKF3_core::getLLH(struct Location &loc) const

Here is the relevant code.

bool NavEKF3_core::getPosNE(Vector2f &posNE) const
{
const struct Location &gpsloc_tmp = _ahrs->get_gps().location();
Vector2f temp2PosNE = location_diff(EKF_origin, gpsloc_tmp);
posNE.x = temp2PosNE.x;
posNE.y = temp2PosNE.y;
return true;
…
}

bool NavEKF3_core::getPosD(float &posD) const
{
const struct Location &gpsloc = _ahrs->get_gps().location();
posD = -gpsloc.alt/100.0;
return filterStatus.flags.vert_pos;
}

bool NavEKF3_core::getHAGL(float &HAGL) const
{
const struct Location &gpsloc = _ahrs->get_gps().location();
HAGL = terrainState + gpsloc.alt/100.0 - posOffsetNED.z;
// If we know the terrain offset and altitude, then we have a valid height above ground estimate
return !hgtTimeout && gndOffsetValid && healthy();
}

bool NavEKF3_core::getLLH(struct Location &loc) const
{
const AP_GPS &gps = AP::gps();

if(validOrigin) {
	const struct Location &gpsloc2 = _ahrs->get_gps().location();
		loc.lat = gpsloc2.lat;
		loc.lng = gpsloc2.lng;
		// Altitude returned is an absolute altitude relative to the WGS-84 spherioid
		loc.alt = gpsloc2.alt;
		loc.flags.relative_alt = 0;
		loc.flags.terrain_alt = 0;
		return true;
	}

It’s very simple and works very well (~5 cm precision)
Good luck
Doron

1 Like