MPC with MSF and ROVIO

This page describes how to run our Multi-Sensor Fusion (MSF), RObust Visual Inertial Odometry (ROVIO) and linear Model Predictive Control frameworks for general use.

First, generate your MPC parameters:

$roscd mav_linear_mpc/resources/
$cp ./linear_mpc_flourish.yaml ./linear_mpc_yourMavName.yaml
$roscd mav_disturbance_observer/resources/
$cp ./disturbance_observer_flourish.yaml ./disturbance_observer_yourMavName.yaml

Run all the ROS nodes:

$roslaunch mav_dji_ros_interface raven_indoor_rovio_nMPC_trj_realsense.launch

You can open the node_manager to check that everything is running as expected, as shown below. Please check that all ROS nodes are identical to yours and running (green circles). From here, you can echo the messages associated with each node.

We provide an MSF configuration file describing measurement noise, measurement delay, and sensor extrinsic configurations (e.g., the transformation between the autopilot IMU and the VI-sensor). This file can be found in mav_dji_ros_interface/dji_interface/cfg/raven/msf_parameters_raven_realsense-2017-05-11-18-42-09.yaml. Re-calibration is not necessary as long as sensor configurations remain the same. If you perform your own MSF calibration, please follow the instructions from Kalibr.

MSF can then be initialized by call ROS service with the following command. Open a terminal on a ground computer.

rosservice call /yourMavName/pose_sensor_rovio/pose_sensor/initialize_msf_scale 1.0

<node pkg="rosservice" type="rosservice" name="initializer" args="call --wait /yourMavName/pose_sensor_rovio/pose_sensor/initialize_msf_scale 1.0"/>

You should see the console output below.

[ INFO] [1494274998.637483500]: Initialize filter with scale 1.000000
[ INFO] [1494274998.637613925]: initial measurement pos:[  -0.0506445 -1.68889e-05    0.0208878] orientation: [-0.493, -0.0701, 0.867, -0.00308]
[ WARN] [1494274998.752131925]: Using simulated core plus fixed diag initial error state covariance.
[ INFO] [1494274998.762309384]: Initializing msf_core (built: May  8 2017)
[ INFO] [1494274998.762451909]: Core parameters:
    fixed_bias:    0
    fuzzythres:    0.1
    noise_acc:    0.083
    noise_accbias:    5e-05
    noise_gyr:    0.0013
    noise_gyrbias:    1.3e-06
 
[ INFO] [1494274998.762850489]: Core init with state:
--------- State at time 4998.65s: ---------
0 : [0-2]     : Matrix<3, 1>         : [-0.00920399  -0.0119434   0.0608539]
1 : [3-5]     : Matrix<3, 1>         : [0 0 0]
2 : [6-9]     : Quaternion (w,x,y,z) : [-0.654, 0.365, -0.34, 0.568]
3 : [10-12]     : Matrix<3, 1>         : [0 0 0]
4 : [13-15]     : Matrix<3, 1>         : [0 0 0]
5 : [16-16]     : Matrix<1, 1>         : [1]
6 : [17-20]     : Quaternion (w,x,y,z) : [1, 0, 0, 0]
7 : [21-23]     : Matrix<3, 1>         : [0 0 0]
8 : [24-27]     : Quaternion (w,x,y,z) : [0.000321, 0.718, -0.696, -0.0107]
9 : [28-30]     : Matrix<3, 1>         : [-0.01573   0.0151  -0.0546]

Then, use rviz view to visualize estimated pose as shown in the following figure.