QuadRotor testBed for localization and mapping

I am trying to build a test bed for trying out localization and mapping algorithms.

Similar projects worth following
I am trying to build a Quadrotor testBed for trying out localization and mapping projects. The current alternatives are very expensive for me which is the reason I came up with this project. My interest lies in developing a testBed that can allow people to focus on the algorithms rather than getting bogged down by the infrastructure required to implement the algorithm.
Quadrotors are very complex but interesting vehicles. I have been a huge admirer of these machines. Owing to my interest in Robotics, I needed to play with certain applications of these machines.
Currently, I feel the way to implement this idea would be to use a ardupilot on a beaglebone blue. Ardupilot is an open source software that can help me work on the specifics of my project rather than dealing with control issues. Along with the beaglebone blue, the copter would comprise of a Scanse sweep Lidar from sparkfun for generating the Occupancy maps. This serves as a platform to conduct reserch on the platform.


This project aims at building a Quadrotor test bed for trying out autonomous algorithms. The quadrotor is a fairly complex machine which can serve as a useful tool to test out new algorithms for your research. I intend to test localization and mapping algorithms on this test bed.


For mobile devices, the ability to navigate the environment is crucial. To navigate a complex environment with unknown obstacles, it becomes paramount to know where the robot is located. This need to know where the robot location is a highly complex subject which can be solved through a variety of algorithms. The goal for the robot can only be achieved if the localization system helps the system navigate without crashing into unknown territory.

Hardware used:

A Quadrotor kit :

Scanse Sweep Lidar:

BeagleBone Blue :

Software used:

ROS on beaglebone blue

Point CLoud library

Ardupilot :


The aim of this project is to implement certain open source ROS algorithms on the test bed. The test bed itself can be assembled using the components. I intend to work on ICP and particle filters on the Quadrotor. The beaglebone blue would be used to run the Ardupilot on it providing motion commands to the rover. I intend to install ROS indigo on the platform so as to simultaneously work on the Ardupilot and ROS. The scans or the pointcloud data would be generated by the Scanse sweep module which have the necessary ROS drivers

Iterative closest point (ICP) is an algorithm employed to minimize the difference between two clouds of points. ICP is often used to reconstruct 2D or 3D surfaces from different scans, to localize robots and achieve optimal path planning.

The experiments that I need to conduct is basically ICP scan matching and registering. I am trying to get the pose of the vehicle using the scan registering process. The pose can help me get the correct position of the vehicle in the unknown environment. To implement the scan matching, I would be using the Point cloud library. Point cloud library is an easy interface for implementing localization algorihtms. ICP implementation from the PCL can help me get the maps necessary for the mapping process.

The position of the copter as derived by the Ardupilot controls can be published on a ROS node. This vealue is then used to filter the value that we get from the scan matching. The updated position can be used as the position of the rotor.


The project deliverables would be a software and hardware solution integrating Ardupilot and ROS for implementing localization and mapping modules from ROS into Ardupilot.

  • 1 × BeagleBone Blue Controller
  • 1 × Quadrotor kit
  • 1 × Scanse Sweep Lidar

  • Extended Kalman Filter

    int-smart03/24/2018 at 03:39 0 comments

    I had explained Kalman filter in the last post. What happens when the motion model and observation model is not linear. In robotics, the linear case occurs rarely. So algorithms that can handle non-linear models can be used to localize. The Extended Kalman filter is one such algorithm.

    The observation and motion model for non-linear case can be represented by:

    Here g and h represent the non-linear model. The other notations are the same as those in Kalman filter equations. After applying the non-linear models to the state x which in our case is a gaussian, we get a distribution that is not a gaussian. We try to approximate this distribution with a gaussian to get the final state. This is done by using Taylor expansion which linearizes the function at the mean.
    Using first-order Taylor expansion at the mean we can approximate the motion and observation model with the following linear model:
    Let the jacobians for motion and observation model be,
    Substituting the above linear model into the Bayes filter equations we get,
    Replacing the Kalman filter equations with the new linear model we get:
    The above equations show the Extended Kalman filter where we just linearized the non-linear modelwith Taylor expansion. This method however performs linearization just at the mean due to which it is not representative of the actual non-linear model. It works well in cases where the non-linearities are moderate.

  • Kalman Filter

    int-smart03/23/2018 at 23:25 0 comments


    Kalman filters can be intrepreted as Bayes filter with the gaussian linear case. There application range from economics to weather forecasting to satellite navigation. If models are not linear the beliefs do not remain gaussian and we can not work with a simple kalman filter.

    A multivariate Gaussians can be described:Let v be a multivariate gaussian distribution:

    The marginal of x can be defined as:
    The conditional of x with respect to y is:
    The kalman filter linear and observation models can be thus represented by:
    where x represent the motion model and z represent the observation model. Here the motion noise epsilon is zero mean gaussian with covariance R and the observation noise is zero mean with Q covariance.
    Substituting these values in the Bayes filter motion and observation models we get,
    The above values can be substituted in the prediction and correction step below:
    to get the final Kalman filter equations described in the algorithm below:
    The first two equations show the predictions for the mean and covariance of the state and the last two equations give us the final belief of the state considering the observations. Once again, all of this is just applicable when we have linear motion and observation model. For non-linear models we consider the Extended Kalman filter that I would be explaining in my next post.

  • SLAM

    int-smart03/16/2018 at 03:14 0 comments

    Simultaneous Localization and Mapping (SLAM):

    SLAM is a probabilistic technique to construct and update maps for navigation while localizing the object pose in the map. The problem appears to be a chicken and egg problem as to localize the vehicle we need a map and to update the map we need to know the pose of the vehicle. This problem can be solved through multiple approximate techniques such as Extended kalman filters and Particle filters.

    The problem can be formulated mathematically as finding:

    Input: robot's controls inputs u and observations z.

    Output: Map of the environment m and the pose of the robot x

    This can be done using bayes filter. Bayes filter is a two step process: Prediction and Correction. 

    Predicton step predicts the state of the vehicle using the motion model, control inputs and previous belief while the correction step reduces the uncertainty using the observation model. The motion model describes the distribution of the current state of the system given the control inputs for this time step and the previous belief of the pose of the vehicle. This can be represented mathematically as:The predicted state of the system is :

    The Sensor model or the Observation model is responsible for reducing the uncertainty that is introduced by the random nature of the problem. The predicted state is:This belief value is the current pose of the system.

    There are multiple ways to implement the Bayes filter:

    • Kalman Filters which deals with gaussian distributions and linear models
    • Particle Filters which deal with non-parametric arbitrary models

    Kalman filters deal with the linear gaussian case. By linear we mean a linear model and by gaussian we mean gaussian noise is associated with the pose and observations.

  • Scan Matching and Registration

    int-smart03/14/2018 at 01:25 0 comments

    ICP (Iterative closest point) is a popular range data processing method that can be used for range registration. Given two sets of slightly overlapping point clouds the ICP computes the registeration of one scan over the other in an iterative fashion.

    Mathematically we can construct the problem as:

    Given two point setwe need to find the translation and rotation that

    If the correct correspondances are known, we can calculate the alignment using SVD. It can be done using first subtracting the value of the center of mass of the two point clouds from the points in the set and then taking the SVD of the accumulation matrix defined by:The rotation matrix is given by

    The registeration algorihtm is briefly described in the following:

    • For each point in the first scan, find the closest point in the second scan.
    • Find the relative position of each scan so that the MSE (Mean squared error) calculated as the mean of the distance between the closest points is minimized.
    • Repeat until the MSE is below certain threshold or stops changing.

View all 4 project logs

  • 1
    1. Preparing the BeagleBone Blue

    Preparing the software environment of BeagleBoneBlue. Installing required packages

    Step 1:

    sudo apt-get install python-pip python-setuptools python-yaml python-distribute

    Step 2:

    sudo apt-get python-docutils python-dateutil python-six

    Step 3:

    sudo apt-get install libconsole-bridge-dev liblz4-dev checkinstall cmake python-empy python-nose libbz2-dev libboost-test-dev libboost-dev libboost-program-options-dev libboost-regex-dev libboost-signals-dev libtinyxml-dev libboost-filesystem-dev libxml2-dev libgtest-dev
  • 2
    2. Intsalling ROS on the blue

     Installing ROS kinetic or Indigo on the Blue

    Step 1:

    sudo sh -c 'echo "deb jessie main" > /etc/apt/sources.list.d/ros-latest.list'

    wget -O - | sudo apt-key add -

    Step 2:
    sudo apt-get update

    sudo apt-get upgrade
    Step 3:
    sudo pip install rosdep rosinstall_generator wstool rosinstall

    Step 4:
    sudo rosdep init
    rosdep update

View all instructions

Enjoy this project?



Similar Projects

Does this project spark your interest?

Become a member to follow this project and never miss any updates