Deleted self.L from the loops if possible and replaced it with KalmanGain
Change-Id: I22ba10ec042b72a91631b5e408429116b9d4b102
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index 401be44..42433e9 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -123,7 +123,6 @@
// compile or have assertion failures at runtime.
const StateFeedbackPlantCoefficients<2, 4, 7> coefficients(
Eigen::Matrix<double, 2, 2>::Identity(),
- Eigen::Matrix<double, 2, 2>::Identity(),
Eigen::Matrix<double, 2, 4>::Identity(),
Eigen::Matrix<double, 7, 2>::Identity(),
Eigen::Matrix<double, 7, 4>::Identity(),
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 82d138a..767ed5a 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -279,6 +279,7 @@
"""
self.L = controls.dplace(self.A.T, self.C.T, poles).T
+
def Update(self, U):
"""Simulates one time step with the provided U."""
#U = numpy.clip(U, self.U_min, self.U_max)
@@ -291,8 +292,6 @@
def CorrectObserver(self, U):
"""Runs the correct step of the observer update."""
- # See if the KalmanGain exists. That lets us avoid inverting A.
- # Otherwise, do the inversion.
if hasattr(self, 'KalmanGain'):
KalmanGain = self.KalmanGain
else:
@@ -301,8 +300,12 @@
def UpdateObserver(self, U):
"""Updates the observer given the provided U."""
+ if hasattr(self, 'KalmanGain'):
+ KalmanGain = self.KalmanGain
+ else:
+ KalmanGain = numpy.linalg.inv(self.A) * self.L
self.X_hat = (self.A * self.X_hat + self.B * U +
- self.L * (self.Y - self.C * self.X_hat - self.D * U))
+ self.A * KalmanGain * (self.Y - self.C * self.X_hat - self.D * U))
def _DumpMatrix(self, matrix_name, matrix, scalar_type):
"""Dumps the provided matrix into a variable called matrix_name.
@@ -352,10 +355,9 @@
if plant_coefficient_type.startswith('StateFeedbackPlant'):
ans.append(self._DumpMatrix('A', self.A, scalar_type))
- ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A), scalar_type))
ans.append(self._DumpMatrix('B', self.B, scalar_type))
ans.append(' return %s'
- '(A, A_inv, B, C, D, U_max, U_min);\n' % (
+ '(A, B, C, D, U_max, U_min);\n' % (
plant_coefficient_type))
elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
ans.append(self._DumpMatrix('A_continuous', self.A_continuous, scalar_type))
@@ -438,8 +440,13 @@
observer_coefficient_type, self.ObserverFunction())]
if observer_coefficient_type.startswith('StateFeedbackObserver'):
- ans.append(self._DumpMatrix('L', self.L, scalar_type))
- ans.append(' return %s(L);\n' % (observer_coefficient_type,))
+ if hasattr(self, 'KalmanGain'):
+ KalmanGain = self.KalmanGain
+ else:
+ KalmanGain = numpy.linalg.inv(self.A) * self.L
+ ans.append(self._DumpMatrix('KalmanGain', KalmanGain, scalar_type))
+ ans.append(' return %s(KalmanGain);\n' % (observer_coefficient_type,))
+
elif observer_coefficient_type.startswith('HybridKalman'):
ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous, scalar_type))
ans.append(self._DumpMatrix('R_continuous', self.R_continuous, scalar_type))
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index b9765fc..30da8f1 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -34,7 +34,6 @@
StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
: A(other.A),
- A_inv(other.A_inv),
B(other.B),
C(other.C),
D(other.D),
@@ -43,16 +42,14 @@
StateFeedbackPlantCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A,
- const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A_inv,
const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> &B,
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)
- : A(A), A_inv(A_inv), B(B), C(C), D(D), U_min(U_min), U_max(U_max) {}
+ : A(A), B(B), C(C), D(D), U_min(U_min), U_max(U_max) {}
const Eigen::Matrix<Scalar, number_of_states, number_of_states> A;
- const Eigen::Matrix<Scalar, number_of_states, number_of_states> A_inv;
const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B;
const Eigen::Matrix<Scalar, number_of_outputs, number_of_states> C;
const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> D;
@@ -87,10 +84,6 @@
return coefficients().A;
}
Scalar A(int i, int j) const { return A()(i, j); }
- const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A_inv() const {
- return coefficients().A_inv;
- }
- Scalar A_inv(int i, int j) const { return A_inv()(i, j); }
const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> &B() const {
return coefficients().B;
}
@@ -276,11 +269,11 @@
struct StateFeedbackObserverCoefficients final {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
- const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> L;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> KalmanGain;
StateFeedbackObserverCoefficients(
- const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &L)
- : L(L) {}
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &KalmanGain)
+ : KalmanGain(KalmanGain) {}
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -299,10 +292,10 @@
::std::swap(coefficients_, other.coefficients_);
}
- const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &L() const {
- return coefficients().L;
+ const Eigen::Matrix<Scalar, number_of_states, number_of_outputs> &KalmanGain() const {
+ return coefficients().KalmanGain;
}
- Scalar L(int i, int j) const { return L()(i, j); }
+ Scalar KalmanGain(int i, int j) const { return KalmanGain()(i, j); }
const Eigen::Matrix<Scalar, number_of_states, 1> &X_hat() const {
return X_hat_;
@@ -326,7 +319,7 @@
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
const Eigen::Matrix<Scalar, number_of_outputs, 1> &Y) {
mutable_X_hat() +=
- plant.A_inv() * L() * (Y - plant.C() * X_hat() - plant.D() * U);
+ KalmanGain() * (Y - plant.C() * X_hat() - plant.D() * U);
}
// Sets the current controller to be index, clamped to be within range.
diff --git a/y2018/control_loops/python/intake.py b/y2018/control_loops/python/intake.py
index ac3e4c1..f29d3b5 100755
--- a/y2018/control_loops/python/intake.py
+++ b/y2018/control_loops/python/intake.py
@@ -103,15 +103,11 @@
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
-
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
self.U_max = numpy.matrix([[12.0]])
self.U_min = numpy.matrix([[-12.0]])
- self.Kff = controls.TwoStateFeedForwards(self.B, self.Q)
-
self.InitializeState()
@@ -201,8 +197,6 @@
self.KalmanGain, self.Q_steady = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
- self.L = self.A * self.KalmanGain
-
# The box formed by U_min and U_max must encompass all possible values,
# or else Austin's code gets angry.
self.U_max = numpy.matrix([[12.0]])
@@ -343,15 +337,20 @@
scenario_plotter.Plot()
# Write the generated constants out to a file.
- if len(argv) != 3:
- glog.fatal('Expected .h file name and .cc file name for the intake.')
+ if len(argv) != 5:
+ glog.fatal('Expected .h file name and .cc file name for intake and delayed_intake.')
else:
- namespaces = ['y2018', 'control_loops', 'superstructure']
+ namespaces = ['y2018', 'control_loops', 'superstructure', 'intake']
intake = Intake('Intake')
loop_writer = control_loop.ControlLoopWriter(
'Intake', [intake], namespaces=namespaces)
loop_writer.Write(argv[1], argv[2])
+ delayed_intake = DelayedIntake('DelayedIntake')
+ loop_writer = control_loop.ControlLoopWriter(
+ 'DelayedIntake', [delayed_intake], namespaces=namespaces)
+ loop_writer.Write(argv[3], argv[4])
+
if __name__ == '__main__':
argv = FLAGS(sys.argv)
glog.init()