A system for enabling autonomous control of a rotorcraft micro-aerial vehicle in indoor and outdoor environments, the system comprising: a sensor fusion module for combining measurements from a plurality of sensors (112, 16, 118, 120, 122, 123) of different modalities to estimate a current state of the rotorcraft micro-aerial vehicle given current and previous measurements from the sensors and a previous estimated state of the rotorcraft micro-aerial vehicle, wherein the sensor fusion module is configured to maintain smoothness in a pose estimate of the vehicle when: one or more sensors provide inaccurate information, when global positioning system measurements are unavailable after a period of availability, or when GPS measurements become available after a period of unavailability, wherein the smoothness in the pose estimate is maintained by transforming a previous global measured GPS constraint on pose (z k-1) to minimize a discrepancy between the transformed global measured GPS constraint on pose (z - k-1) and a previous pose estimate (s k-1), wherein the sensors include a global positioning system receiver, an inertial measurement unit, a laser scanner, and a camera and wherein the sensor fusion module is configured to convert relative measurements into measurements that depend on augmented states, where the augmented states are current states augmented with past states of the rotorcraft micro-aerial vehicle measured by the sensors; and a trajectory generator for generating a time parameterized trajectory for controlling a trajectory of the rotorcraft micro-aerial vehicle based on the estimated current state and a goal or a waypoint input provided by either a user or a high level planner of the system, wherein the high level planner is configured to generate a sequence of desired 3D positions and yaw angles and to simplify a resulting path into a minimum set of waypoints, wherein the trajectory generator is configured to transform the waypoints into an odometry frame using pose estimates from visual SLAM. a sensor fusion module for combining measurements from a plurality of sensors (112, 16, 118, 120, 122, 123) of different modalities to estimate a current state of the rotorcraft micro-aerial vehicle given current and previous measurements from the sensors and a previous estimated state of the rotorcraft micro-aerial vehicle, wherein the sensor fusion module is configured to maintain smoothness in a pose estimate of the vehicle when: one or more sensors provide inaccurate information, when global positioning system measurements are unavailable after a period of availability, or when GPS measurements become available after a period of unavailability, wherein the smoothness in the pose estimate is maintained by transforming a previous global measured GPS constraint on pose (z k-1) to minimize a discrepancy between the transformed global measured GPS constraint on pose (z - k-1) and a previous pose estimate (s k-1), wherein the sensors include a global positioning system receiver, an inertial measurement unit, a laser scanner, and a camera and wherein the sensor fusion module is configured to convert relative measurements into measurements that depend on augmented states, where the augmented states are current states augmented with past states of the rotorcraft micro-aerial vehicle measured by the sensors; and a trajectory generator for generating a time parameterized trajectory for controlling a trajectory of the rotorcraft micro-aerial vehicle based on the estimated current state and a goal or a waypoint input provided by either a user or a high level planner of the system, wherein the high level planner is configured to generate a sequence of desired 3D positions and yaw angles and to simplify a resulting path into a minimum set of waypoints, wherein the trajectory generator is configured to transform the waypoints into an odometry frame using pose estimates from visual SLAM.