Adding Object Tracking

I'm adding simple object tracking to the Arducopter code.  I have come up with a system running on a BeagleBoard that has a downward pointing camera that captures an image, finds the object in the image, calculates it's centroid and sends that data as a MAVLink message to the ArduPilot Mega.  This all works fine.

I then take that centroid information and use an algorithm similar to the existing optical flow one to find the actual offset in centimeters in the x and y directions.  I then calculate the distance and bearing to the object.  I then use functions very similar to those used for waypoint navigation:

- calc_XY_velocity() I've copied and modified to calculate the relative velocity to the object (this is the first place I'm not sure about)

- calc_location_error() is replaced by the object x and y distances, which give me values just like long_error and lat_error in centimeters

- calc_desired_speed() is calculated the same way

- calc_nav_rate() I've copied and modified to use the relative speed to the object.  It uses the same x/y_rate_error, pi_loiter_lon/lat, pid_nav_lon/lat, and nav_lon/lat calculations as calc_nav_rate().  The PID values are the same as for waypoint (same objects are used).

- calc_loiter_pitch_roll() is unmodified

- I have extended the ALT_HOLD mode with object tracking (if an object is detected) and use these parameters:

            yaw_mode         = YAW_HOLD;
            roll_pitch_mode = ROLL_PITCH_AUTO;
            throttle_mode     = THROTTLE_AUTO;

So roll/pitch get updated as with waypoint navigation.

I have been testing it but I get strange results.  When the copter detects an object (verified through logs), I see that it flies away from it.  For example, if the object is in front of the copter, the copter pitches left and flies away.  Other times, it pitches forward and keeps going fast, sometimes out of control.

I've tripled checked my code and I don't think I've swapped an x/y or lat/lon and I believe all the units are correct.  The implementation is similar to what is there already.  I've logged values extensively, analyzed them, and see that indeed large values are being produced, but I'm not sure why yet.  I am kind of stuck and wanted to know your thoughts on what to try.


1) What are the units of nav_lon/nav_lat?  I have tried to figure it out, but it is not clear.

2) Should I use the GPS speed instead of the relative speed?  I would like to not have to depend on a GPS fix for object tracking...

3) Does my approach seem sound?

My log is attached with the column headings inserted at the top.  The scenario is described in the log.  Object tracking starts at row 812/817.

Thank you in advance!

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –


  • Dear Noah,

    We are a team of students from the university of Southampton. We are making an autonomous surface vessel capable of crossing the atlantic. We are at a good stage with the hullform and superstructure design and we are now going into electronics.

    We now think that the best solution, in order to have satellite communications and AIS object avoidance, is to have a Beaglebone Black connected to an Ardupilot. The APM would do the lower level computations of manoeuvring the vessel, where the BBB would be connected to the other systems and would do higher levels calculations (i.e. object avoidance, etc..). We believe that this system is ideal as we can use the Ardupilot library and just program the remaining parts in ROS and Python for the BBB (which would then communicate with the APM, over Mavlink and serial ports, to give new way points in case of avoiding something).

    Can you tell me more about the BBB and APM interface? Also how hard do you think it is to change the Ardurover code to recieve info from BBB? (considering that i only know how to program in Python and a little bit in ROS.

  • Nice job!

    Have you considered to follow the helicopter with the vision system?
    You can put some light on the helicopter to make it easier to detect.

    It would be a good system for tracking in two dimensions?
    I have a BeagleBoard and I like to try ...

  • At this point, I have things kind of working, but when the copter sees the object, it only occasionally follows it.  Most of the time, though, it just wobbles quite a bit.  Does this indicate too high of PI values?  I am using P=3.0 and I=0.1, from waypoint navigation.  Any suggestions as to which to adjust first?  Next time I'm out, I will try messing with them.

  • Are the values long_error and lat_error calculated by calc_location_error() in units of centimeters or something else?  This comment in the function is confusing:

        Becuase we are using lat and lon to do our distance errors here's a quick chart:
        100     = 1m
        1000     = 11m     = 36 feet
        1800     = 19.80m = 60 feet
        3000     = 33m
        10000     = 111m

  • Developer

    Nav_lon and nav_lat are degrees * 100  before the translation into copter space. So It's pitch and roll if the copter was pointed north.

    I'll re-read your post in a few days when I get some more time. Very interesting, thanks.


This reply was deleted.


Gremsy liked Gremsy's profile
Mar 12
DIY Robocars via Twitter
RT @chr1sa: Donkeycar 4.4 released with tons of new features, including path learning (useful with GPS outdoors), better Web and Lidar supp…
Nov 27, 2022
DIY Robocars via Twitter
RT @NXP: We are already biting our nails in anticipation of the #NXPCupEMEA challenge! 😉 Did you know there are great cash prizes to be won…
Nov 24, 2022
DIY Robocars via Twitter
RT @gclue_akira: レースまであと3日。今回のコースは激ムズかも。あと一歩 #jetracer
Nov 24, 2022
DIY Robocars via Twitter
UC Berkeley's DIY robocar program
Nov 24, 2022
DIY Robocars via Twitter
RT @chr1sa: The next @DIYRobocars autonomous car race at @circuitlaunch will be on Sat, Dec 10. Thrills, spills and a Brazilian BBQ. Fun…
Nov 24, 2022
DIY Robocars via Twitter
RT @arthiak_tc: Donkey car platform ... Still training uses behavioral cloning #TCXpo #diyrobocar @OttawaAVGroup
Nov 20, 2022
DIY Robocars via Twitter
RT @emurmur77: Points for style. @donkeycar racing in @diyrobocars at @UCSDJacobs thanks @chr1sa for taking the video.…
Nov 20, 2022
DIY Robocars via Twitter
RT @SmallpixelCar: Going to @diyrobocars race at @UCSDJacobs
Nov 8, 2022
DIY Robocars via Twitter
RT @SmallpixelCar: Race @diyrobocars at @UCSDJacobs thanks @chr1sa for taking the video.
Nov 8, 2022
DIY Robocars via Twitter
RT @PiWarsRobotics: Presenting: the Hacky Racers Robotic Racing Series in collaboration with #PiWars. Find out more and register your inter…
Oct 23, 2022
DIY Robocars via Twitter
RT @Hacky_Racers: There will be three classes at this event: A4, A2, and Hacky Racer! A4 and A2 are based around UK paper sizing and existi…
Oct 23, 2022
DIY Robocars via Twitter
Oct 23, 2022
DIY Robocars via Twitter
Oct 19, 2022
DIY Robocars via Twitter
Oct 18, 2022
DIY Robocars via Twitter
RT @NeaveEng: Calling all UK based folks interested in @diyrobocars, @f1tenth, @donkey_car, and similar robot racing competitions! @hacky_r…
Oct 13, 2022