Add support for multiple cycles of delay for U
Falcons are best modeled as having even more delay. Sigh
Change-Id: Ia108f8cbd81572245c91e6727b0c7e46b6c15843
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/capped_test_plant.h b/frc971/control_loops/capped_test_plant.h
index 65bc7cc..f4286cd 100644
--- a/frc971/control_loops/capped_test_plant.h
+++ b/frc971/control_loops/capped_test_plant.h
@@ -29,4 +29,4 @@
} // namespace frc971
} // namespace control_loops
-#endif // FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
\ No newline at end of file
+#endif // FRC971_CONTROL_LOOPS_CAPPED_TEST_PLANT_H_
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index fd39359..46e6a05 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -39,7 +39,7 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> &C,
const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
- const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min, bool delayed_u)
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min, size_t delayed_u)
: A_continuous(A_continuous),
B_continuous(B_continuous),
C(C),
@@ -55,7 +55,7 @@
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
- const bool delayed_u;
+ const size_t delayed_u;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -230,7 +230,7 @@
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
P_steady_state;
- const bool delayed_u;
+ const size_t delayed_u;
HybridKalmanCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
@@ -238,7 +238,7 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs>
&R_continuous,
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
- &P_steady_state, bool delayed_u)
+ &P_steady_state, size_t delayed_u)
: Q_continuous(Q_continuous),
R_continuous(R_continuous),
P_steady_state(P_steady_state), delayed_u(delayed_u) {
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index f431983..1649dd2 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -266,7 +266,7 @@
name: string, The name of the loop to use when writing the C++ files.
"""
self._name = name
- self.delayed_u = False
+ self.delayed_u = 0
@property
def name(self):
@@ -291,7 +291,7 @@
self.X = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
self.Y = self.C * self.X
self.X_hat = numpy.matrix(numpy.zeros((self.A.shape[0], 1)))
- self.last_U = numpy.matrix(numpy.zeros((self.B.shape[1], 1)))
+ self.last_U = numpy.matrix(numpy.zeros((self.B.shape[1], max(1, self.delayed_u))))
def PlaceControllerPoles(self, poles):
"""Places the controller poles.
@@ -314,19 +314,21 @@
def Update(self, U):
"""Simulates one time step with the provided U."""
#U = numpy.clip(U, self.U_min, self.U_max)
- if self.delayed_u:
- self.X = self.A * self.X + self.B * self.last_U
- self.Y = self.C * self.X + self.D * self.last_U
- self.last_U = U.copy()
+ if self.delayed_u > 0:
+ self.X = self.A * self.X + self.B * self.last_U[:, -1]
+ self.Y = self.C * self.X + self.D * self.last_U[:, -1]
+ self.last_U[:, 1:] = self.last_U[:, 0:-1]
+ self.last_U[:, 0] = U.copy()
else:
self.X = self.A * self.X + self.B * U
self.Y = self.C * self.X + self.D * U
def PredictObserver(self, U):
"""Runs the predict step of the observer update."""
- if self.delayed_u:
- self.X_hat = (self.A * self.X_hat + self.B * self.last_U)
- self.last_U = U.copy()
+ if self.delayed_u > 0:
+ self.X_hat = (self.A * self.X_hat + self.B * self.last_U[:, -1])
+ self.last_U[:, 1:] = self.last_U[:, 0:-1]
+ self.last_U[:, 0] = U.copy()
else:
self.X_hat = (self.A * self.X_hat + self.B * U)
@@ -336,9 +338,9 @@
KalmanGain = self.KalmanGain
else:
KalmanGain = numpy.linalg.inv(self.A) * self.L
- if self.delayed_u:
+ if self.delayed_u > 0:
self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat -
- self.D * self.last_U)
+ self.D * self.last_U[:, -1])
else:
self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat -
self.D * U)
@@ -396,7 +398,7 @@
ans.append(self._DumpMatrix('U_max', self.U_max, scalar_type))
ans.append(self._DumpMatrix('U_min', self.U_min, scalar_type))
- delayed_u_string = "true" if self.delayed_u else "false"
+ delayed_u_string = str(self.delayed_u)
if plant_coefficient_type.startswith('StateFeedbackPlant'):
ans.append(self._DumpMatrix('A', self.A, scalar_type))
ans.append(self._DumpMatrix('B', self.B, scalar_type))
@@ -492,7 +494,7 @@
'%s %s {\n' % (observer_coefficient_type, self.ObserverFunction())
]
- delayed_u_string = "true" if self.delayed_u else "false"
+ delayed_u_string = str(self.delayed_u)
if observer_coefficient_type.startswith('StateFeedbackObserver'):
if hasattr(self, 'KalmanGain'):
KalmanGain = self.KalmanGain
@@ -540,9 +542,10 @@
def PredictHybridObserver(self, U, dt):
self.Discretize(dt)
- if self.delayed_u:
- self.X_hat = self.A * self.X_hat + self.B * self.last_U
- self.last_U = U.copy()
+ if self.delayed_u > 0:
+ self.X_hat = self.A * self.X_hat + self.B * self.last_U[:, -1]
+ self.last_U[:, 1:] = self.last_U[:, 0:-1]
+ self.last_U[:, 0] = U.copy()
else:
self.X_hat = self.A * self.X_hat + self.B * U
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 53cd6a2..1ea94d9 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -12,6 +12,7 @@
#if defined(__linux__)
#include "aos/logging/logging.h"
+#include "glog/logging.h"
#endif
#include "aos/macros.h"
@@ -49,7 +50,7 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_max,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min,
- const std::chrono::nanoseconds dt, bool delayed_u)
+ const std::chrono::nanoseconds dt, size_t delayed_u)
: A(A),
B(B),
C(C),
@@ -71,7 +72,7 @@
// useful for modeling a control loop cycle where you sample, compute, and
// then queue the outputs to be ready to be executed when the next cycle
// happens.
- const bool delayed_u;
+ const size_t delayed_u;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -85,6 +86,16 @@
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
&&coefficients)
: coefficients_(::std::move(coefficients)), index_(0) {
+ if (coefficients_.size() > 1u) {
+ for (size_t i = 1; i < coefficients_.size(); ++i) {
+ if (coefficients_[i]->delayed_u != coefficients_[0]->delayed_u) {
+ abort();
+ }
+ }
+ }
+ last_U_ = Eigen::Matrix<Scalar, number_of_inputs, Eigen::Dynamic>(
+ number_of_inputs,
+ std::max(static_cast<size_t>(1u), coefficients_[0]->delayed_u));
Reset();
}
@@ -175,15 +186,27 @@
}
}
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> last_U(
+ size_t index = 0) const {
+ return last_U_.template block<number_of_inputs, 1>(0, index);
+ }
+
// Computes the new X and Y given the control input.
void Update(const Eigen::Matrix<Scalar, number_of_inputs, 1> &U) {
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
- if (coefficients().delayed_u) {
- X_ = Update(X(), last_U_);
- UpdateY(last_U_);
- last_U_ = U;
+ if (coefficients().delayed_u > 0) {
+#if defined(__linux__)
+ DCHECK_EQ(static_cast<ssize_t>(coefficients().delayed_u), last_U_.cols());
+#endif
+ X_ = Update(X(), last_U(coefficients().delayed_u - 1));
+ UpdateY(last_U(coefficients().delayed_u - 1));
+ for (int i = coefficients().delayed_u; i > 1; --i) {
+ last_U_.template block<number_of_inputs, 1>(0, i - 1) =
+ last_U_.template block<number_of_inputs, 1>(0, i - 2);
+ }
+ last_U_.template block<number_of_inputs, 1>(0, 0) = U;
} else {
X_ = Update(X(), U);
UpdateY(U);
@@ -210,7 +233,7 @@
private:
Eigen::Matrix<Scalar, number_of_states, 1> X_;
Eigen::Matrix<Scalar, number_of_outputs, 1> Y_;
- Eigen::Matrix<Scalar, number_of_inputs, 1> last_U_;
+ Eigen::Matrix<Scalar, number_of_inputs, Eigen::Dynamic> last_U_;
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
@@ -310,14 +333,14 @@
// useful for modeling a control loop cycle where you sample, compute, and
// then queue the outputs to be ready to be executed when the next cycle
// happens.
- const bool delayed_u;
+ const size_t delayed_u;
StateFeedbackObserverCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_outputs>
&KalmanGain,
const Eigen::Matrix<Scalar, number_of_states, number_of_states> &Q,
const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R,
- bool delayed_u)
+ size_t delayed_u)
: KalmanGain(KalmanGain), Q(Q), R(R), delayed_u(delayed_u) {}
};
@@ -331,7 +354,10 @@
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
&&observers)
- : coefficients_(::std::move(observers)) {}
+ : coefficients_(::std::move(observers)) {
+ last_U_ = Eigen::Matrix<Scalar, number_of_inputs, Eigen::Dynamic>(
+ number_of_inputs, std::max(static_cast<size_t>(1u), coefficients().delayed_u));
+ }
StateFeedbackObserver(StateFeedbackObserver &&other)
: X_hat_(other.X_hat_), last_U_(other.last_U_), index_(other.index_) {
@@ -349,8 +375,9 @@
}
Eigen::Matrix<Scalar, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
- const Eigen::Matrix<Scalar, number_of_inputs, 1> &last_U() const {
- return last_U_;
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> last_U(
+ size_t index = 0) const {
+ return last_U_.template block<number_of_inputs, 1>(0, index);
}
void Reset(StateFeedbackPlant<number_of_states, number_of_inputs,
@@ -363,9 +390,14 @@
number_of_outputs, Scalar> *plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds /*dt*/) {
- if (plant->coefficients().delayed_u) {
- mutable_X_hat() = plant->Update(X_hat(), last_U_);
- last_U_ = new_u;
+ if (plant->coefficients().delayed_u > 0) {
+ mutable_X_hat() =
+ plant->Update(X_hat(), last_U(coefficients().delayed_u - 1));
+ for (int i = coefficients().delayed_u; i > 1; --i) {
+ last_U_.template block<number_of_inputs, 1>(0, i - 1) =
+ last_U_.template block<number_of_inputs, 1>(0, i - 2);
+ }
+ last_U_.template block<number_of_inputs, 1>(0, 0) = new_u;
} else {
mutable_X_hat() = plant->Update(X_hat(), new_u);
}
@@ -406,7 +438,7 @@
private:
// Internal state estimate.
Eigen::Matrix<Scalar, number_of_states, 1> X_hat_;
- Eigen::Matrix<Scalar, number_of_inputs, 1> last_U_;
+ Eigen::Matrix<Scalar, number_of_inputs, Eigen::Dynamic> last_U_;
int index_ = 0;
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<