Add support for modeling the output delay
The FPGA adds a 1 cycle delay to everything. We should model that
in our kalman filter for more accuracy during highly dynamic situations
(like a catapult fire).
Change-Id: I41efa81c6ab474904d6eb02c85a5e238c498f226
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 0170d7c..fd39359 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -28,7 +28,8 @@
C(other.C),
D(other.D),
U_min(other.U_min),
- U_max(other.U_max) {}
+ U_max(other.U_max),
+ delayed_u(other.delayed_u) {}
StateFeedbackHybridPlantCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
@@ -38,13 +39,14 @@
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)
+ const Eigen::Matrix<Scalar, number_of_inputs, 1> &U_min, bool delayed_u)
: A_continuous(A_continuous),
B_continuous(B_continuous),
C(C),
D(D),
U_min(U_min),
- U_max(U_max) {}
+ U_max(U_max),
+ delayed_u(delayed_u) {}
const Eigen::Matrix<Scalar, number_of_states, number_of_states> A_continuous;
const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B_continuous;
@@ -52,6 +54,8 @@
const Eigen::Matrix<Scalar, number_of_outputs, number_of_inputs> D;
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
+
+ const bool delayed_u;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -226,16 +230,20 @@
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
P_steady_state;
+ const bool delayed_u;
+
HybridKalmanCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_states>
&Q_continuous,
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)
+ &P_steady_state, bool delayed_u)
: Q_continuous(Q_continuous),
R_continuous(R_continuous),
- P_steady_state(P_steady_state) {}
+ P_steady_state(P_steady_state), delayed_u(delayed_u) {
+ CHECK(!delayed_u) << ": Delayed hybrid filters aren't supported yet.";
+ }
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
diff --git a/frc971/control_loops/hybrid_state_feedback_loop_test.cc b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
index c866457..147a84f 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop_test.cc
+++ b/frc971/control_loops/hybrid_state_feedback_loop_test.cc
@@ -34,7 +34,7 @@
B_continuous(1, 0) = 443.75;
B_continuous(2, 0) = 0.0;
return StateFeedbackHybridPlantCoefficients<3, 1, 1>(
- A_continuous, B_continuous, C, D, U_max, U_min);
+ A_continuous, B_continuous, C, D, U_max, U_min, false);
}
StateFeedbackControllerCoefficients<3, 1, 1>
@@ -74,7 +74,7 @@
P_steady_state(2, 1) = 0.015962850974712596;
P_steady_state(2, 2) = 0.0019821816120708254;
return HybridKalmanCoefficients<3, 1, 1>(Q_continuous, R_continuous,
- P_steady_state);
+ P_steady_state, false);
}
StateFeedbackHybridPlant<3, 1, 1> MakeIntegralShooterPlant() {
@@ -128,7 +128,7 @@
Eigen::Matrix<double, 7, 4>::Identity(),
Eigen::Matrix<double, 4, 1>::Constant(1),
Eigen::Matrix<double, 4, 1>::Constant(-1),
- frc971::controls::kLoopFrequency);
+ frc971::controls::kLoopFrequency, false);
// Build a plant.
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<2, 4, 7>>>
@@ -153,7 +153,7 @@
v_observer.emplace_back(new StateFeedbackObserverCoefficients<2, 4, 7>(
Eigen::Matrix<double, 2, 7>::Identity(),
Eigen::Matrix<double, 2, 2>::Identity(),
- Eigen::Matrix<double, 7, 7>::Identity()));
+ Eigen::Matrix<double, 7, 7>::Identity(), false));
StateFeedbackObserver<2, 4, 7> observer(std::move(v_observer));
StateFeedbackLoop<2, 4, 7> test_loop(
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 72d3297..f431983 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -266,6 +266,7 @@
name: string, The name of the loop to use when writing the C++ files.
"""
self._name = name
+ self.delayed_u = False
@property
def name(self):
@@ -290,6 +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)))
def PlaceControllerPoles(self, poles):
"""Places the controller poles.
@@ -312,12 +314,21 @@
def Update(self, U):
"""Simulates one time step with the provided U."""
#U = numpy.clip(U, self.U_min, self.U_max)
- self.X = self.A * self.X + self.B * U
- self.Y = self.C * self.X + self.D * U
+ 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()
+ 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."""
- self.X_hat = (self.A * self.X_hat + self.B * U)
+ if self.delayed_u:
+ self.X_hat = (self.A * self.X_hat + self.B * self.last_U)
+ self.last_U = U.copy()
+ else:
+ self.X_hat = (self.A * self.X_hat + self.B * U)
def CorrectObserver(self, U):
"""Runs the correct step of the observer update."""
@@ -325,16 +336,12 @@
KalmanGain = self.KalmanGain
else:
KalmanGain = numpy.linalg.inv(self.A) * self.L
- self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat - self.D * U)
-
- def UpdateObserver(self, U):
- """Updates the observer given the provided U."""
- if hasattr(self, 'KalmanGain'):
- KalmanGain = self.KalmanGain
+ if self.delayed_u:
+ self.X_hat += KalmanGain * (self.Y - self.C * self.X_hat -
+ self.D * self.last_U)
else:
- KalmanGain = numpy.linalg.inv(self.A) * self.L
- self.X_hat = (self.A * self.X_hat + self.B * U + self.A * KalmanGain *
- (self.Y - self.C * self.X_hat - self.D * U))
+ self.X_hat += 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.
@@ -389,6 +396,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"
if plant_coefficient_type.startswith('StateFeedbackPlant'):
ans.append(self._DumpMatrix('A', self.A, scalar_type))
ans.append(self._DumpMatrix('B', self.B, scalar_type))
@@ -396,7 +404,7 @@
' const std::chrono::nanoseconds dt(%d);\n' % (self.dt * 1e9))
ans.append(
' return %s'
- '(A, B, C, D, U_max, U_min, dt);\n' % (plant_coefficient_type))
+ '(A, B, C, D, U_max, U_min, dt, %s);\n' % (plant_coefficient_type, delayed_u_string))
elif plant_coefficient_type.startswith('StateFeedbackHybridPlant'):
ans.append(
self._DumpMatrix('A_continuous', self.A_continuous,
@@ -405,8 +413,8 @@
self._DumpMatrix('B_continuous', self.B_continuous,
scalar_type))
ans.append(' return %s'
- '(A_continuous, B_continuous, C, D, U_max, U_min);\n' %
- (plant_coefficient_type))
+ '(A_continuous, B_continuous, C, D, U_max, U_min, %s);\n' %
+ (plant_coefficient_type, delayed_u_string))
else:
glog.fatal('Unsupported plant type %s', plant_coefficient_type)
@@ -484,6 +492,7 @@
'%s %s {\n' % (observer_coefficient_type, self.ObserverFunction())
]
+ delayed_u_string = "true" if self.delayed_u else "false"
if observer_coefficient_type.startswith('StateFeedbackObserver'):
if hasattr(self, 'KalmanGain'):
KalmanGain = self.KalmanGain
@@ -496,8 +505,8 @@
ans.append(self._DumpMatrix('KalmanGain', KalmanGain, scalar_type))
ans.append(self._DumpMatrix('Q', Q, scalar_type))
ans.append(self._DumpMatrix('R', R, scalar_type))
- ans.append(' return %s(KalmanGain, Q, R);\n' %
- (observer_coefficient_type, ))
+ ans.append(' return %s(KalmanGain, Q, R, %s);\n' %
+ (observer_coefficient_type, delayed_u_string))
elif observer_coefficient_type.startswith('HybridKalman'):
ans.append(
@@ -510,8 +519,8 @@
self._DumpMatrix('P_steady_state', self.P_steady_state,
scalar_type))
ans.append(
- ' return %s(Q_continuous, R_continuous, P_steady_state);\n' %
- (observer_coefficient_type, ))
+ ' return %s(Q_continuous, R_continuous, P_steady_state, %s);\n' %
+ (observer_coefficient_type, delayed_u_string))
else:
glog.fatal('Unsupported observer type %s',
observer_coefficient_type)
@@ -531,7 +540,12 @@
def PredictHybridObserver(self, U, dt):
self.Discretize(dt)
- self.X_hat = self.A * self.X_hat + self.B * U
+ if self.delayed_u:
+ self.X_hat = self.A * self.X_hat + self.B * self.last_U
+ self.last_U = U.copy()
+ else:
+ self.X_hat = self.A * self.X_hat + self.B * U
+
self.P = (self.A * self.P * self.A.T + self.Q)
def CorrectHybridObserver(self, U):
diff --git a/frc971/control_loops/python/drivetrain.py b/frc971/control_loops/python/drivetrain.py
index 4069036..264b4a6 100644
--- a/frc971/control_loops/python/drivetrain.py
+++ b/frc971/control_loops/python/drivetrain.py
@@ -544,7 +544,8 @@
for _ in range(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
- drivetrain.UpdateObserver(U)
+ drivetrain.CorrectObserver(U)
+ drivetrain.PredictObserver(U)
drivetrain.Update(U + numpy.matrix([[1.0], [1.0]]))
close_loop_left.append(drivetrain.X[0, 0])
close_loop_right.append(drivetrain.X[2, 0])
@@ -588,7 +589,8 @@
for _ in range(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
- drivetrain.UpdateObserver(U)
+ drivetrain.CorrectObserver(U)
+ drivetrain.PredictObserver(U)
drivetrain.Update(U)
close_loop_left.append(drivetrain.X[0, 0])
close_loop_right.append(drivetrain.X[2, 0])
@@ -612,7 +614,8 @@
for _ in range(200):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
- drivetrain.UpdateObserver(U)
+ drivetrain.CorrectObserver(U)
+ drivetrain.PredictObserver(U)
drivetrain.Update(U)
close_loop_left.append(drivetrain.X[0, 0])
close_loop_right.append(drivetrain.X[2, 0])
@@ -633,7 +636,8 @@
for _ in range(300):
U = numpy.clip(drivetrain.K * (R - drivetrain.X_hat), drivetrain.U_min,
drivetrain.U_max)
- drivetrain.UpdateObserver(U)
+ drivetrain.CorrectObserver(U)
+ drivetrain.PredictObserver(U)
drivetrain.Update(U)
close_loop_left.append(drivetrain.X[0, 0])
close_loop_right.append(drivetrain.X[2, 0])
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index f591847..0e4be89 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -39,7 +39,8 @@
D(other.D),
U_min(other.U_min),
U_max(other.U_max),
- dt(other.dt) {}
+ dt(other.dt),
+ delayed_u(other.delayed_u) {}
StateFeedbackPlantCoefficients(
const Eigen::Matrix<Scalar, number_of_states, number_of_states> &A,
@@ -48,8 +49,15 @@
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)
- : A(A), B(B), C(C), D(D), U_min(U_min), U_max(U_max), dt(dt) {}
+ const std::chrono::nanoseconds dt, bool delayed_u)
+ : A(A),
+ B(B),
+ C(C),
+ D(D),
+ U_min(U_min),
+ U_max(U_max),
+ dt(dt),
+ delayed_u(delayed_u) {}
const Eigen::Matrix<Scalar, number_of_states, number_of_states> A;
const Eigen::Matrix<Scalar, number_of_states, number_of_inputs> B;
@@ -58,6 +66,12 @@
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_min;
const Eigen::Matrix<Scalar, number_of_inputs, 1> U_max;
const std::chrono::nanoseconds dt;
+
+ // If true, this adds a single cycle output delay model to the plant. This is
+ // 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;
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -78,6 +92,7 @@
::std::swap(coefficients_, other.coefficients_);
X_.swap(other.X_);
Y_.swap(other.Y_);
+ last_U_.swap(other.last_U_);
}
virtual ~StateFeedbackPlant() {}
@@ -143,6 +158,7 @@
void Reset() {
X_.setZero();
Y_.setZero();
+ last_U_.setZero();
}
// Assert that U is within the hardware range.
@@ -164,8 +180,14 @@
// Powers outside of the range are more likely controller bugs than things
// that the plant should deal with.
CheckU(U);
- X_ = Update(X(), U);
- UpdateY(U);
+ if (coefficients().delayed_u) {
+ X_ = Update(X(), last_U_);
+ UpdateY(last_U_);
+ last_U_ = U;
+ } else {
+ X_ = Update(X(), U);
+ UpdateY(U);
+ }
}
// Computes the new Y given the control input.
@@ -188,6 +210,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_;
::std::vector<::std::unique_ptr<StateFeedbackPlantCoefficients<
number_of_states, number_of_inputs, number_of_outputs, Scalar>>>
@@ -283,12 +306,19 @@
const Eigen::Matrix<Scalar, number_of_states, number_of_states> Q;
const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> R;
+ // If true, this adds a single cycle output delay model to the plant. This is
+ // 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;
+
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)
- : KalmanGain(KalmanGain), Q(Q), R(R) {}
+ const Eigen::Matrix<Scalar, number_of_outputs, number_of_outputs> &R,
+ bool delayed_u)
+ : KalmanGain(KalmanGain), Q(Q), R(R), delayed_u(delayed_u) {}
};
template <int number_of_states, int number_of_inputs, int number_of_outputs,
@@ -304,7 +334,7 @@
: coefficients_(::std::move(observers)) {}
StateFeedbackObserver(StateFeedbackObserver &&other)
- : X_hat_(other.X_hat_), index_(other.index_) {
+ : X_hat_(other.X_hat_), last_U_(other.last_U_), index_(other.index_) {
::std::swap(coefficients_, other.coefficients_);
}
@@ -322,13 +352,19 @@
void Reset(StateFeedbackPlant<number_of_states, number_of_inputs,
number_of_outputs, Scalar> * /*loop*/) {
X_hat_.setZero();
+ last_U_.setZero();
}
void Predict(StateFeedbackPlant<number_of_states, number_of_inputs,
number_of_outputs, Scalar> *plant,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &new_u,
::std::chrono::nanoseconds /*dt*/) {
- mutable_X_hat() = plant->Update(X_hat(), new_u);
+ if (plant->coefficients().delayed_u) {
+ mutable_X_hat() = plant->Update(X_hat(), last_U_);
+ last_U_ = new_u;
+ } else {
+ mutable_X_hat() = plant->Update(X_hat(), new_u);
+ }
}
void Correct(const StateFeedbackPlant<number_of_states, number_of_inputs,
@@ -366,6 +402,7 @@
private:
// Internal state estimate.
Eigen::Matrix<Scalar, number_of_states, 1> X_hat_;
+ Eigen::Matrix<Scalar, number_of_inputs, 1> last_U_;
int index_ = 0;
::std::vector<::std::unique_ptr<StateFeedbackObserverCoefficients<
diff --git a/y2014/control_loops/python/shooter.py b/y2014/control_loops/python/shooter.py
index 710233b..c3e8d86 100755
--- a/y2014/control_loops/python/shooter.py
+++ b/y2014/control_loops/python/shooter.py
@@ -217,7 +217,8 @@
U = sprung_shooter.K * (R - sprung_shooter.X_hat)
U = ClipDeltaU(sprung_shooter, voltage, U)
sprung_shooter.Y = raw_sprung_shooter.Y + 0.01
- sprung_shooter.UpdateObserver(U)
+ sprung_shooter.CorrectObserver(U)
+ sprung_shooter.PredictObserver(U)
voltage += U
raw_sprung_shooter.Update(voltage)
close_loop_x.append(raw_sprung_shooter.X[0, 0] * 10)
@@ -240,7 +241,8 @@
U = shooter.K * (R - shooter.X_hat)
U = ClipDeltaU(shooter, voltage, U)
shooter.Y = raw_shooter.Y + 0.01
- shooter.UpdateObserver(U)
+ shooter.CorrectObserver(U)
+ shooter.PredictObserver(U)
voltage += U
raw_shooter.Update(voltage)
close_loop_x.append(raw_shooter.X[0, 0] * 10)