Dynamic System Identification (via VirtualRC)
This page describes how to identify the underlying the dynamics of the MAV (i.e., roll, pitch, yaw rate, vertical velocity) by using only on-board sensor measurements (no motion capture device). To complete this section, you need:
- Logitec F710 Gamepad (Parts List, ID 20)
The table below summarizes the inputs and outputs that we need to measure and the transfer functions that we want to identify:
Note: We only need the z-axis component from the topics marked by the star. yourMavName will be prefixed as the ROS namespace of all topics.
1. Set-up the N1 autopilot for publishing on-board velocity estimation.
By default, the DJI on-board SDK node publishes IMU messages, whereas vertical velocity requires a setting to be made through the DJI adjustment application, as shown in the figure below. We only enable "Velocity(Ground frame)" at 50Hz. Under-the-hood, IMU and barometer readings are fused by a filter to obtain higher-frequency and more accurate estimates. Unfortunately, this source code in not publicly available.
2. Prepare a joypad and plug its USB dongle into the NUC.
Once you run the command below, you will be able to see the published joy topic, as shown below.
$roslaunch dji_sdk Onboard_joy_for_systemID.launch
Then, you will be able to see the ROS nodes below. Note that falcon2 here corresponds to "yourMavName", as an example.
It's recommended to check the topics that are subscribed and published to using node_manager. The figure below shows joy message (left) published by the joy_for_sys_id node and command/roll_pitch_yawrate_thrust (right) published by the rotors_joy_interface_for_m100 node.
In order to send command messages to the N1 autopilot, we require access permissions. This can be done by running a keyboard node in the yourMavName namespace on the ground station:
$ROS_NAMESPACE=falcon2 rosrun keyboard keyboard
This will cause a tiny window to pop up where keyboard events can be received. It's important to activate this window before sending keyboard commands; otherwise, no keydown messages will be sent.
Now, you can turn on the motors and check whether your commanded messages are sent to the autopilot:
- Turn on the motors by performing the action shown in the following figure (courtesy of DJI) on the light bridge 2 (your transmitter):
- Change/make sure the transmitter is set to "F" mode.
- Click the keyboard window and type "b" to obtain control.
- Check the virtual command topic
dji_sdk/vc_cmdto see whether roll, pitch, yaw rate, and thrust commands are publishing as shown below. For safety reasons, it is recommended to remove all propellers during this test.
While sending the command through the joypad, you should also hear some vibrating sounds from the motors. If it seems that your commands are not executed, please check your "app_id" and "enc_key" from the Onboard_joy_for_systemID.launch file.
3. Perform manual flights using a joypad and record relevant data for system identification.
To perform dynamic system identification, we need two sets of data: input and output. The input is our joypad command (dji_sdk/vc_cmd) and the outputs are the IMU (dji_sdk/imu) and velocity (dji_sdk/velocity). A motion capture device (VICON) is unnecessary. The figure below shows the experimental set-up used for this task. The joypad sends virtual RC commands to the NUC, which has a plugged-in bluetooth receiver.
An example can be obtained by running the following command. Note that the package does not have to be in your workspace as it is independent from ROS.
$git clone https://github.com/ethz-asl/mav_system_identification -b DJI_M100_sysID
1. First-order dynamics
A MATLAB script that estimates the underlying roll and pitch dynamics of the MAV is located in ~/indigo_catkin_ws/src/mav_system_identification/falcon2_sysID/falcon2_sysid_roll_pitch_onboard.m. Open MATLAB and run the script. You will be shown 9 figures. As shown below, we collected two datasets (one for training a model and the other for testing it)
We can see the estimated pitch and roll moments shown below. We use a 1st order model that provides a ~70% fit.
The MATLAB console output also clearly motivates the use of MPC, as indicated below in bold:
The roll model is estimated using experiment 1 and validated on data from experiment 2
The roll model fits the validation data with 77.2798 %
The pitch model is estimated using experiment 1 and validated on data from experiment 2
The pitch model fits the validation data with 75.301 %
Roll estimated transfer function is:
ans =
From input "roll_{cmd}" to output "roll":
4.349
--------
s + 3.95
Continuous-time transfer function.
roll tau=0.253, gain=1.101
Pitch estimated transfer function is:
ans =
From input "pitch_{cmd}" to output "pitch":
4.102
---------
s + 3.739
Continuous-time transfer function.
pitch tau=0.267, gain=1.097
2. Second-order dynamics
The second-order dynamics model for the disturbance observed in MPC is estimated in a similar manner. First, change Line 216 in falcon2_sysid_roll_pitch_onboard.m from np=1 to np=2. This represents the number of poles in a system.
Run the script and you will be able to see the figures below, accompanied by the MATLAB console output.
The roll model is estimated using experiment 1 and validated on data from experiment 2
The roll model fits the validation data with 86.2258 %
The pitch model is estimated using experiment 1 and validated on data from experiment 2
The pitch model fits the validation data with 87.0233 %
Roll estimated transfer function is:
ans =
From input "roll_{cmd}" to output "roll":
38.27
---------------------
s^2 + 7.287 s + 40.16
Continuous-time transfer function.
roll omega=6.337, gain=0.953, damping=0.575
Pitch estimated transfer function is:
ans =
From input "pitch_{cmd}" to output "pitch":
39.24
--------------------
s^2 + 6.94 s + 41.45
Continuous-time transfer function.
pitch omega=6.438, gain=0.947, damping=0.539
A MATLAB script that estimates the underlying yaw rate dynamics of the MAV is located in ~/indigo_catkin_ws/src/mav_system_identification/falcon2_sysID/falcon2_sysID/falcon2_sysid_yaw_rate_onboard.m. First- and second-order dynamics system identification will be performed, as in the previous sub-section.
1. First-order dynamics
Running the script will produce the figures shown below.
The yawrate model is estimated using experiment 1 and validated on data from experiment 2
The yawrate model fits the validation data with 75.9647 %
yawrate estimated transfer function is:
ans =
From input "yawrate_{cmd}" to output "yawrate":
2.689
---------
s + 3.111
Continuous-time transfer function.
yawrate gain=0.865, tau=0.321
2. Second-order dynamics
As in the previous sub-section, the second-order dynamics model of the MPC disturbance observer will be estimated. Change line 144 in falcon2_sysid_yaw_rate_onboard.m from np=1 to np=2. Run the script.
The yawrate model is estimated using experiment 1 and validated on data from experiment 2
The yawrate model fits the validation data with 77.9918 %
yawrate estimated transfer function is:
ans =
From input "yawrate_{cmd}" to output "yawrate":
67.41
---------------------
s^2 + 24.82 s + 79.55
Continuous-time transfer function.
yawrate omega=8.919, gain=0.847, damping=1.391
Only the first-order dynamics are needed for vertical velocity. The corresponding MATLAB script is located in ~/indigo_catkin_ws/src/mav_system_identification/falcon2_sysID/falcon2_sysid_height_onboard.m. Running the script will produce the figures below.
The thrust model fits the validation data with 79.69 %
Thrust estimated transfer function is:
ans =
From input "thrust_{cmd}" to output "thrust":
2.594
---------
s + 2.199
Continuous-time transfer function.
thrust gain=1.180, tau=0.455
These parameters need to be filled in in the MPC set-up files:
~/indigo_catkin_ws/src/mav_control_rw/mav_linear_mpc_m100/resources/linear_mpc_yourMavName.yaml~/indigo_catkin_ws/src/mav_control_rw/mav_disturbance_observer/resources/disturbance_observer_yourMavName.yaml
As example, you can find two files for our test platform, falcon2, at:
~/indigo_catkin_ws/src/mav_control_rw/mav_linear_mpc_m100/resources/linear_mpc_falcon2.yaml~/indigo_catkin_ws/src/mav_control_rw/mav_disturbance_observer/resources/disturbance_observer_falcon2.yaml
4. Finally, measure the total mass of the MAV using a scale.
In our case, this is 2895.3g.
⇒ You can now move on to performing a step response in your first autonomous flight.