Quantitatively comparing the estimated trajectory with
the groundtruth, however, is not an easy task. There are
two major difficulties.
First, the estimated trajectory and
the groundtruth are usually expressed in different reference
frames, and, therefore, cannot be compared directly.
Second,
a trajectory consists of the states at many different times and,
therefore, is high-dimensional data.
Thus, how to summarize
the information of the whole trajectory into concise accuracy
metrics is not trivial.
To address the first problem, the
estimated trajectory requires to be properly transformed into
the same reference frame as the groundtruth, which is often
called trajectory alignment.
To address the second problem,
meaningful error metrics need to be used and their properties
well understood.
Roughly speaking, we integrate the raw IMU
measurements to get the relative rotation ∆ R̃ ik , velocity
∆ṽ ik and position ∆p̃ ik between two states x i and x k , and
the integration is formulated to be independent of the states
(except for the biases) so that re-integration is not needed
when the states change (e.g., during optimization iterations).
The corresponding measurement model is
(6) has infinite solutions that have the same minimum
cost. The reason is that the predicted measurements f (X)
are invariant to certain transformations g(·) of the parameter,
namely $f (X) = f (X’ )$ with $X’ = g(X)$. Since the measure-
ments z̃ are constant, the cost function (7) is also invariant to
classPX4CtrlFSM{public:// somedataprivate:State_tstate;// Should only be changed in PX4CtrlFSM::process() function!AutoTakeoffLand_ttakeoff_land;// ---- control related ----Desired_State_tget_hover_des();Desired_State_tget_cmd_des();// ---- auto takeoff/land ----voidmotors_idling(constImu_Data_t&imu,Controller_Output_t&u);voidland_detector(constState_tstate,constDesired_State_t&des,constOdom_Data_t&odom);// Detect landing voidset_start_pose_for_takeoff_land(constOdom_Data_t&odom);Desired_State_tget_rotor_speed_up_des(constros::Timenow);Desired_State_tget_takeoff_land_des(constdoublespeed);// ---- tools ----voidset_hov_with_odom();voidset_hov_with_rc();booltoggle_offboard_mode(boolon_off);// It will only try to toggle once, so not blocked.booltoggle_arm_disarm(boolarm);// It will only try to toggle once, so not blocked.voidreboot_FCU();voidpublish_bodyrate_ctrl(constController_Output_t&u,constros::Time&stamp);voidpublish_attitude_ctrl(constController_Output_t&u,constros::Time&stamp);voidpublish_trigger(constnav_msgs::Odometry&odom_msg);};
structAutoTakeoffLand_t{boollanded{true};ros::Timetoggle_takeoff_land_time;std::pair<bool,ros::Time>delay_trigger{std::pair<bool,ros::Time>(false,ros::Time(0))};Eigen::Vector4dstart_pose;staticconstexprdoubleMOTORS_SPEEDUP_TIME=3.0;// motors idle running for 3 seconds before takeoffstaticconstexprdoubleTAKEOFF_SPEEDUP_TIME=0.5;// Time to speed up during takeoff font stagestaticconstexprdoubleDELAY_TRIGGER_TIME=2.0;// Time to be delayed when reach at target height};