Position Hold with VISION_POSITION_ESTIMATE and ATT_POS_MOCAP for indoor localization

ArduCopter
, , ,
10

Hi Patrick,

It passes the lag into the EKF by adjusting the timestamp. so we subtract the lag from the timestamp and pass it in. This is the commit that adjust the timestamp.

1 Like
11

@rmackay9 ok I see, other question , is Paul wip on EKF3 is complete or we still have to use EKF2?

12

Copter fly in circle mode using current master mentioned by @rmackay9 , parrot bebop using ardupilot, optitrack motion capture system. use ATT_POS_MOCAP to send external nav data to copter through wifi. COMPASS_USE=0, COMPASS_USE2=0, COMPASS_USE3, EK2_GPS_TYPE=3

3 Likes
13

@chobitsfan,
This is fantastic! Can we blog this on ardupilot.org? It’d be best if it was you but I’ll do it if you’re too busy.

14

Patrick,
The EKF3 changes still need testing, peer review before going into master. I’m not sure of the timing but I think @priseborough has it in hand.

16

Can we blog this on ardupilot.org? It’d be best if it was you but I’ll do it if you’re too busy.

Hi @rmackay9 Thank you. It is done here. I am not a native speaker of English, so I am sorry if my article is difficult to understand.

1 Like
17

Really great Chobitsfan, thanks!

18

@vkurtz I’m interested in testing external nav messages in simulation. I cloned the ardupilot_gazebo package from your link into the src dir of an existing catkin workspace, ran catkin_make, but I am unable to do “roslaunch ardupilot_gazebo mavros_opitrack.launch”. It looks like ROS won’t recognize the package (won’t tab complete, etc.) Any ideas?

19

@gs1mm0ns,
Have you run the following?

source [catkin_ws]/devel/setup.bash
source /opt/ros/kinetic/setup.bash

I believe that’s necessary to properly initialize a new package

20

@vkurtz Yes, ran those and still encountered the issue. However it’s resolved now, I ran rospack list and that seemed to get ros to update its package list. Thanks!

21

Here is a video showing my results in Arducopter SITL, flying in guided mode using the VISION_LAG parameter binary. 250 ms seems to be the sweet spot for me. I am also noticing variance from the set position, mostly in the roll axis. I have followed @vkurtz’s parameter recommendations. Posting this in case anyone has additional recommendations to get the quad to track the external localization input more tightly.

22

@gs1mm0ns I’ve had some luck improving the tracking performance in simulation by reducing the roll rate D gain: i.e. setting ATC_RAT_RLL_D = 0.0001

23

@vkurtz Thank you, I’ll play with the roll rate D parameter and see if I can get improved behavior.

24

@vkurtz I am looking at the code in your ardupilot_gazebo on github and I don’t understand if the values on this line for set_home_position are correct:


q should be in this form [w, x, y, z] and [1, 0, 0, 0] should be the null-rotation, why do you use [0, 0, 0, 1] instead?

25

@anbello you are totally right about that, I was mixed up since ROS often uses [x, y, z, w]. It shouldn’t matter too much, since I believe the home position could be set with any valid orientation, but I’ve made the correction to the github repo. Thanks!

26

@vkurtz this suggestion improved my performance in simulation immensely, thank you!

27

Hey @JustFineD, Where and how in the arducopter code base do you override the aforementioned functions?

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
29

Thanks Doron! I am gonna give this a try.

30

000063.BIN (162.7 KB)
Hi everyone,
I am trying to use aruco markers to localize my drone but I haven’t been successful in doing so. Here I attach a log file to one of the experiments in which I was just trying to give a takeoff command via QGroundControl. I had set the take off altitude to 1m but the drone took off to a height of nearly 2 m and then lost height and fell to the ground. Can anyone please take a look at my logfiles and tell me what is wrong? @rmackay9 @anbello @chobitsfan @vkurtz

@vkurtz I seem to have similar delays between vision message and ekf’s estimate, I wanted to know if the vision_lag related change in the code is in the master? I doesn’t seem to be.
thanks.