Model the hybrid KF as having 1 cycle of U delay.
Change-Id: I71e86095da408467445b88c036fd2b9acf4cea1d
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 891aaa6..41812f2 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -326,6 +326,7 @@
Y_.setZero();
A_.setZero();
B_.setZero();
+ DelayedU_.setZero();
UpdateAB(::aos::controls::kLoopFrequency);
}
@@ -344,10 +345,19 @@
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
- X_ = Update(X(), U, dt);
- Y_ = C() * X() + D() * U;
+ ::aos::robot_state.FetchLatest();
+
+ Eigen::Matrix<double, number_of_inputs, 1> current_U =
+ DelayedU_ * (::aos::robot_state.get()
+ ? ::aos::robot_state->voltage_battery / 12.0
+ : 1.0);
+ X_ = Update(X(), current_U);
+ Y_ = C() * X() + D() * current_U;
+ DelayedU_ = U;
}
+ Eigen::Matrix<double, number_of_inputs, 1> DelayedU_;
+
Eigen::Matrix<double, number_of_states, 1> Update(
const Eigen::Matrix<double, number_of_states, 1> X,
const Eigen::Matrix<double, number_of_inputs, 1> &U,