Ardrone indoor SLAM & navigation

Using recursive least-squares (fusion by PTAM+IMU) to caculate scale info of LSD-SLAM

GitHub :

Hypha-ROS/hypharos_ardrone_navigation

Brief introduction

This project uses recursive least squares algorithm to caculate the transfer function of tum_ardrone/pose (in real size) and LSD_slam/pose(nonscale). After recursive least square converges completely, we used this transfer function to convert nonscale point cloud map from LSD_slam to real world’s map which is used for navigation.

System Diagram

TUM_Ardrone

LSD_SLAM

Hypharos_Ardrone

coordinate transform

Least-squares

consider y = Ax

where A ∈ Rm×n is (strictly) skinny(overdetermined set of linear equations )

define error r = Ax − y

find x = xls that minimizes ∥r∥

Recursive least-squares

In our case ---->

recursively caculate least-squares (approximate) solution

Rotation(quaternion) :

M is quaternion multiplication matrix R is quaternion rotation matrix

Positon :