MPC Tracker - odometry input

[dookei]

Hi Tomas, thanks for your help with the previous integration from the MRS MPC Tracker on non-MRS project. I was going to post this in the same issue but I though this already another issue ;-) So I have been testing / debugging / playing you name it ;-) with the MPC tracker. At the moment I am loading a simple trajectory to track, as you can see below:

Once I startTrajectoryTracking() the mpc starts streaming the predicted trajectory.Until here, everything is working as expected. Well I mean the mpc tracker is working as supposed to.

The issue(s): When I call the update() method with the odometry being passed as reference the start point from the predicted trajectory is not being adjusted. It basically makes the MPC tracker follow the trajectory but the fed odometry has no impact on it. At the moment I have a timer that is calling this every 100ms and feeding it with the odometry / pose from a mavros topic.

mrs_msgs::PositionCommand::ConstPtr tracker_output_cmd;
mrs_msgs::AttitudeCommand::ConstPtr last_attitude_cmd;
mrs_msgs::UavState uav_state;
uav_state.pose.position = _current_pose.pose.position;
uav_state.pose.orientation = _current_pose.orientation;
uav_state.header.frame_id = "uav1/utm_origin";
mrs_msgs::UavState::ConstPtr        uav_state_const_ptr(std::make_unique<mrs_msgs::UavState>(uav_state));
tracker_output_cmd = Hailon_MPC_Tracker::update(uav_state_const_ptr, last_attitude_cmd);

You have any idea what I might be missing ?

I would like to make the tracker dependent on the UAV position and/or velocity. In case I have a slower drone and this is unable to keep track with the predicted time-trajectory. Is it possible to make the mpc tracker dependent on that instead of time based ? Greetings, Pedro

[klaxalk]

Hey, so you are not supposed to use the current odometry for more than a mere initialization of the tracker (during activation). Since the activation, the tracker is supposed to live its own life with the MPC only having a direct influence on the simulated model, see the paper for more info. The simulated model behaves perfectly so the the MPC will actually move it as you would expect. The output of the tracker should actually by the state of the simulated model.

[klaxalk]

Maybe you are still looking at the MPC Tracker as at a controller, which it is not. Yes, It uses an MPC controller inside itself, but it can't control your UAV directly. For that, I recommend, e.g., the SE(3) geometric tracking controller, which is very good at following the reference that is generated by the MPC Tracker. Actually, the SE(3) can deal with all the nonlinearities of the UAV quite easily, wheres, designing an MPC to do that aswell would be a very difficult problem.

[dookei]

Hi, I must have missed something in the beginning then. I guess I was assuming wrong all the way. But shouldn't the tracker generate a predicted trajectory with basis on the current position from the drone?

[dookei]

Hi, so I made a few changes of my own and now I believe I have the desired output for my case scenario. Might not be the same as currently implemented but this was the behavior I was aiming for(in my setup) with the MPC tracker. There are still a few things that I need to add( like dynamic route planning based on current pose) but is getting close. :-D

[klaxalk]

Hey, ye so we do not "input" the current odometry to the MPC tracker because the tracker is not responsible for controlling a real UAV. The tracker outputs states of the UAV translational dynamics that are the "reference" for where the UAV is supposed to be. The UAV is all the time somewhere around the reference and minimizing the difference between the reference (the MPC Tracker's output) and the current states is the job of a "Feedback controller". What might confuse you is that the MPC Tracker contains an MPC controller. But mind that the MPC controller only controls an ideal model without any disturbances. And thanks to the ideal model being part of a simulated feedback loop with the MPC, we can produce an "ideal reference" by sampling the states of the simulated model. And since we are talking about an idea point-mass model, the MPC does not need to account for steady-state errors or nonlinearities, which you should do in the case of a real UAV.

With this mechanism, the reference produced by the tracker provides appropriate feedforward action (acceleration, jerk), and provides a feasible reference in position and velocity. Moreover, the MPC can optimally track even infeasible reference trajectories. All of this for generating a reference for a feedback (and feedforward) controller.

So I guess if you wish to use the Tracker as a controller (meaning you would only use, e.g., the acceleration output of the Tracker), then you can supply the current UAV odometry as the initial state. But don't forget to implement some form of integral feedback to minimize steady-state errors.