To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit fb9a47b1 authored by beuchatp's avatar beuchatp
Browse files

check estimation error

parent f3d16543
......@@ -617,7 +617,7 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
{
// Control output
Eigen::Matrix<float,12,1> u = centralized_controller.K * estimator_state_prev;
Eigen::Matrix<float,12,1> u = -centralized_controller.K * estimator_state_prev;
// Estimator update
estimator_state_prev = centralized_controller.Phi * estimator_state_prev + centralized_controller.Gamma * y_to_controller;
......@@ -1306,6 +1306,12 @@ void ControllerStateReceivedCallback(const CustomButton& commandReceived)
ROS_INFO("[STUDENT CONTROLLER] Button 0 received in controller.");
ROS_INFO("[STUDENT CONTROLLER] Controller State: Motors off.");
// RESET
overlapping_estimator_state_prev.setZero();
estimator_state_prev_only_agent1.setZero();
estimator_state_prev.setZero();
// set measurement vector to zero
y_to_controller.setZero();
controller_state = custom_button_index;
break;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment