Added a hybrid KF for the shooter.
Change-Id: If3ba2e6978773aef2e63c9bfeb0cc6e2dec483d5
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 8f8b381..7272d48 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -109,9 +109,10 @@
'state_feedback_loop.h',
],
deps = [
- '//third_party/eigen',
+ '//aos/common/controls:control_loop',
'//aos/common/logging',
'//aos/common:macros',
+ '//third_party/eigen',
],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2e680a3..aa6cd60 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -151,7 +151,7 @@
rate, angle, down_estimator_.X_hat(0, 0), down_estimator_.X_hat(1, 0));
down_U_(0, 0) = rate;
}
- down_estimator_.UpdateObserver(down_U_);
+ down_estimator_.UpdateObserver(down_U_, ::aos::controls::kLoopFrequency);
// TODO(austin): Signal the current gear to both loops.
@@ -292,7 +292,7 @@
last_left_voltage_ = left_voltage;
last_right_voltage_ = right_voltage;
- kf_.UpdateObserver(U);
+ kf_.UpdateObserver(U, ::aos::controls::kLoopFrequency);
}
void DrivetrainLoop::Zero(
diff --git a/frc971/control_loops/python/control_loop.py b/frc971/control_loops/python/control_loop.py
index 77fd905..7301390 100644
--- a/frc971/control_loops/python/control_loop.py
+++ b/frc971/control_loops/python/control_loop.py
@@ -16,7 +16,9 @@
class ControlLoopWriter(object):
- def __init__(self, gain_schedule_name, loops, namespaces=None, write_constants=False):
+ def __init__(self, gain_schedule_name, loops, namespaces=None,
+ write_constants=False, plant_type='StateFeedbackPlant',
+ observer_type='StateFeedbackObserver'):
"""Constructs a control loop writer.
Args:
@@ -25,6 +27,8 @@
in order.
namespaces: array[string], a list of names of namespaces to nest in
order. If None, the default will be used.
+ plant_type: string, The C++ type of the plant.
+ observer_type: string, The C++ type of the observer.
"""
self._gain_schedule_name = gain_schedule_name
self._loops = loops
@@ -40,6 +44,8 @@
['} // namespace %s' % name for name in reversed(self._namespaces)])
self._constant_list = []
+ self._plant_type = plant_type
+ self._observer_type = observer_type
def AddConstant(self, constant):
"""Adds a constant to write.
@@ -62,13 +68,17 @@
self.WriteHeader(header_file)
self.WriteCC(os.path.basename(header_file), cc_file)
- def _GenericType(self, typename):
+ def _GenericType(self, typename, extra_args=None):
"""Returns a loop template using typename for the type."""
num_states = self._loops[0].A.shape[0]
num_inputs = self._loops[0].B.shape[1]
num_outputs = self._loops[0].C.shape[0]
- return '%s<%d, %d, %d>' % (
- typename, num_states, num_inputs, num_outputs)
+ if extra_args is not None:
+ extra_args = ', ' + extra_args
+ else:
+ extra_args = ''
+ return '%s<%d, %d, %d%s>' % (
+ typename, num_states, num_inputs, num_outputs, extra_args)
def _ControllerType(self):
"""Returns a template name for StateFeedbackController."""
@@ -76,19 +86,20 @@
def _ObserverType(self):
"""Returns a template name for StateFeedbackObserver."""
- return self._GenericType('StateFeedbackObserver')
+ return self._GenericType(self._observer_type)
def _LoopType(self):
"""Returns a template name for StateFeedbackLoop."""
- return self._GenericType('StateFeedbackLoop')
+ extra_args = '%s, %s' % (self._PlantType(), self._ObserverType())
+ return self._GenericType('StateFeedbackLoop', extra_args)
def _PlantType(self):
"""Returns a template name for StateFeedbackPlant."""
- return self._GenericType('StateFeedbackPlant')
+ return self._GenericType(self._plant_type)
def _PlantCoeffType(self):
"""Returns a template name for StateFeedbackPlantCoefficients."""
- return self._GenericType('StateFeedbackPlantCoefficients')
+ return self._GenericType(self._plant_type + 'Coefficients')
def _ControllerCoeffType(self):
"""Returns a template name for StateFeedbackControllerCoefficients."""
@@ -96,7 +107,7 @@
def _ObserverCoeffType(self):
"""Returns a template name for StateFeedbackObserverCoefficients."""
- return self._GenericType('StateFeedbackObserverCoefficients')
+ return self._GenericType(self._observer_type + 'Coefficients')
def WriteHeader(self, header_file, double_appendage=False, MoI_ratio=0.0):
"""Writes the header file to the file named header_file.
@@ -116,11 +127,11 @@
fd.write('\n\n')
for loop in self._loops:
- fd.write(loop.DumpPlantHeader())
+ fd.write(loop.DumpPlantHeader(self._PlantCoeffType()))
fd.write('\n')
fd.write(loop.DumpControllerHeader())
fd.write('\n')
- fd.write(loop.DumpObserverHeader())
+ fd.write(loop.DumpObserverHeader(self._ObserverCoeffType()))
fd.write('\n')
fd.write('%s Make%sPlant();\n\n' %
@@ -152,7 +163,7 @@
fd.write(self._namespace_start)
fd.write('\n\n')
for loop in self._loops:
- fd.write(loop.DumpPlant())
+ fd.write(loop.DumpPlant(self._PlantCoeffType()))
fd.write('\n')
for loop in self._loops:
@@ -160,7 +171,7 @@
fd.write('\n')
for loop in self._loops:
- fd.write(loop.DumpObserver())
+ fd.write(loop.DumpObserver(self._ObserverCoeffType()))
fd.write('\n')
fd.write('%s Make%sPlant() {\n' %
@@ -292,44 +303,45 @@
return ''.join(ans)
- def DumpPlantHeader(self):
+ def DumpPlantHeader(self, plant_coefficient_type):
"""Writes out a c++ header declaration which will create a Plant object.
Returns:
string, The header declaration for the function.
"""
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return 'StateFeedbackPlantCoefficients<%d, %d, %d> Make%sPlantCoefficients();\n' % (
- num_states, num_inputs, num_outputs, self._name)
+ return '%s Make%sPlantCoefficients();\n' % (
+ plant_coefficient_type, self._name)
- def DumpPlant(self):
+ def DumpPlant(self, plant_coefficient_type):
"""Writes out a c++ function which will create a PlantCoefficients object.
Returns:
string, The function which will create the object.
"""
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ['StateFeedbackPlantCoefficients<%d, %d, %d>'
- ' Make%sPlantCoefficients() {\n' % (
- num_states, num_inputs, num_outputs, self._name)]
+ ans = ['%s Make%sPlantCoefficients() {\n' % (
+ plant_coefficient_type, self._name)]
- ans.append(self._DumpMatrix('A', self.A))
- ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
- ans.append(self._DumpMatrix('A_continuous', self.A_continuous))
- ans.append(self._DumpMatrix('B', self.B))
- ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
ans.append(self._DumpMatrix('C', self.C))
ans.append(self._DumpMatrix('D', self.D))
ans.append(self._DumpMatrix('U_max', self.U_max))
ans.append(self._DumpMatrix('U_min', self.U_min))
- ans.append(' return StateFeedbackPlantCoefficients<%d, %d, %d>'
- '(A, A_inv, A_continuous, B, B_continuous, C, D, U_max, U_min);\n' % (
- num_states, num_inputs, num_outputs))
+ if plant_coefficient_type.startswith('StateFeedbackPlant'):
+ ans.append(self._DumpMatrix('A', self.A))
+ ans.append(self._DumpMatrix('A_inv', numpy.linalg.inv(self.A)))
+ ans.append(self._DumpMatrix('B', self.B))
+ ans.append(' return %s'
+ '(A, A_inv, 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))
+ ans.append(self._DumpMatrix('B_continuous', self.B_continuous))
+ ans.append(' return %s'
+ '(A_continuous, B_continuous, C, D, U_max, U_min);\n' % (
+ plant_coefficient_type))
+ else:
+ glog.fatal('Unsupported plant type %s', plant_coefficient_type)
+
ans.append('}\n')
return ''.join(ans)
@@ -381,33 +393,62 @@
ans.append('}\n')
return ''.join(ans)
- def DumpObserverHeader(self):
+ def DumpObserverHeader(self, observer_coefficient_type):
"""Writes out a c++ header declaration which will create a Observer object.
Returns:
string, The header declaration for the function.
"""
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- return 'StateFeedbackObserverCoefficients<%d, %d, %d> %s;\n' % (
- num_states, num_inputs, num_outputs, self.ObserverFunction())
+ return '%s %s;\n' % (
+ observer_coefficient_type, self.ObserverFunction())
- def DumpObserver(self):
+ def DumpObserver(self, observer_coefficient_type):
"""Returns a c++ function which will create a Observer object.
Returns:
string, The function which will create the object.
"""
- num_states = self.A.shape[0]
- num_inputs = self.B.shape[1]
- num_outputs = self.C.shape[0]
- ans = ['StateFeedbackObserverCoefficients<%d, %d, %d> %s {\n' % (
- num_states, num_inputs, num_outputs, self.ObserverFunction())]
+ ans = ['%s %s {\n' % (
+ observer_coefficient_type, self.ObserverFunction())]
- ans.append(self._DumpMatrix('L', self.L))
+ if observer_coefficient_type.startswith('StateFeedbackObserver'):
+ ans.append(self._DumpMatrix('L', self.L))
+ ans.append(' return %s(L);\n' % (observer_coefficient_type,))
+ elif observer_coefficient_type.startswith('HybridKalman'):
+ ans.append(self._DumpMatrix('Q_continuous', self.Q_continuous))
+ ans.append(self._DumpMatrix('R_continuous', self.R_continuous))
+ ans.append(self._DumpMatrix('P_steady_state', self.P_steady_state))
+ ans.append(' return %s(Q_continuous, R_continuous, P_steady_state);\n' % (
+ observer_coefficient_type,))
- ans.append(' return StateFeedbackObserverCoefficients<%d, %d, %d>'
- '(L);\n' % (num_states, num_inputs, num_outputs))
ans.append('}\n')
return ''.join(ans)
+
+class HybridControlLoop(ControlLoop):
+ def __init__(self, name):
+ super(HybridControlLoop, self).__init__(name=name)
+
+ def Discritize(self, dt):
+ [self.A, self.B, self.Q, self.R] = \
+ controls.kalmd(self.A_continuous, self.B_continuous,
+ self.Q_continuous, self.R_continuous, dt)
+
+ def PredictHybridObserver(self, U, dt):
+ self.Discritize(dt)
+ 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):
+ Y_bar = self.Y - self.C * self.X_hat
+ C_t = self.C.T
+ S = self.C * self.P * C_t + self.R
+ self.KalmanGain = self.P * C_t * numpy.linalg.inv(S)
+ self.X_hat = self.X_hat + self.KalmanGain * Y_bar
+ self.P = (numpy.eye(len(self.A)) - self.KalmanGain * self.C) * self.P
+
+ def InitializeState(self):
+ super(HybridControlLoop, self).InitializeState()
+ if hasattr(self, 'Q_steady_state'):
+ self.P = self.Q_steady_state
+ else:
+ self.P = numpy.matrix(numpy.zeros((self.A.shape[0], self.A.shape[0])))
diff --git a/frc971/control_loops/python/controls.py b/frc971/control_loops/python/controls.py
index 211b478..7b0317d 100644
--- a/frc971/control_loops/python/controls.py
+++ b/frc971/control_loops/python/controls.py
@@ -151,6 +151,49 @@
return K, P
+
+def kalmd(A_continuous, B_continuous, Q_continuous, R_continuous, dt):
+ """Converts a continuous time kalman filter to discrete time.
+
+ Args:
+ A_continuous: The A continuous matrix
+ B_continuous: the B continuous matrix
+ Q_continuous: The continuous cost matrix
+ R_continuous: The R continuous matrix
+ dt: Timestep
+
+ The math for this is from:
+ https://www.mathworks.com/help/control/ref/kalmd.html
+
+ Returns:
+ The discrete matrices of A, B, Q, and R.
+ """
+ # TODO(austin): Verify that the dimensions make sense.
+ number_of_states = A_continuous.shape[0]
+ number_of_inputs = B_continuous.shape[1]
+ M = numpy.zeros((len(A_continuous) + number_of_inputs,
+ len(A_continuous) + number_of_inputs))
+ M[0:number_of_states, 0:number_of_states] = A_continuous
+ M[0:number_of_states, number_of_states:] = B_continuous
+ M_exp = scipy.linalg.expm(M * dt)
+ A_discrete = M_exp[0:number_of_states, 0:number_of_states]
+ B_discrete = numpy.matrix(M_exp[0:number_of_states, number_of_states:])
+ Q_continuous = (Q_continuous + Q_continuous.T) / 2.0
+ R_continuous = (R_continuous + R_continuous.T) / 2.0
+ M = numpy.concatenate((-A_continuous, Q_continuous), axis=1)
+ M = numpy.concatenate(
+ (M, numpy.concatenate((numpy.matrix(
+ numpy.zeros((number_of_states, number_of_states))),
+ numpy.transpose(A_continuous)), axis = 1)), axis = 0)
+ phi = numpy.matrix(scipy.linalg.expm(M*dt))
+ phi12 = phi[0:number_of_states, number_of_states:(2*number_of_states)]
+ phi22 = phi[number_of_states:2*number_of_states, number_of_states:2*number_of_states]
+ Q_discrete = phi22.T * phi12
+ Q_discrete = (Q_discrete + Q_discrete.T) / 2.0
+ R_discrete = R_continuous / dt
+ return (A_discrete, B_discrete, Q_discrete, R_discrete)
+
+
def TwoStateFeedForwards(B, Q):
"""Computes the feed forwards constant for a 2 state controller.
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 2a34de3..6d05444 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -7,9 +7,12 @@
#include <memory>
#include <utility>
#include <vector>
+#include <chrono>
#include "Eigen/Dense"
+#include "unsupported/Eigen/MatrixFunctions"
+#include "aos/common/controls/control_loop.h"
#include "aos/common/logging/logging.h"
#include "aos/common/macros.h"
@@ -32,9 +35,7 @@
StateFeedbackPlantCoefficients(const StateFeedbackPlantCoefficients &other)
: A(other.A),
A_inv(other.A_inv),
- A_continuous(other.A_continuous),
B(other.B),
- B_continuous(other.B_continuous),
C(other.C),
D(other.D),
U_min(other.U_min),
@@ -43,30 +44,16 @@
StateFeedbackPlantCoefficients(
const Eigen::Matrix<double, number_of_states, number_of_states> &A,
const Eigen::Matrix<double, number_of_states, number_of_states> &A_inv,
- const Eigen::Matrix<double, number_of_states, number_of_states>
- &A_continuous,
const Eigen::Matrix<double, number_of_states, number_of_inputs> &B,
- const Eigen::Matrix<double, number_of_states, number_of_inputs>
- &B_continuous,
const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
- : A(A),
- A_inv(A_inv),
- A_continuous(A_continuous),
- B(B),
- B_continuous(B_continuous),
- C(C),
- D(D),
- U_min(U_min),
- U_max(U_max) {}
+ : A(A), A_inv(A_inv), B(B), C(C), D(D), U_min(U_min), U_max(U_max) {}
const Eigen::Matrix<double, number_of_states, number_of_states> A;
const Eigen::Matrix<double, number_of_states, number_of_states> A_inv;
- const Eigen::Matrix<double, number_of_states, number_of_states> A_continuous;
const Eigen::Matrix<double, number_of_states, number_of_inputs> B;
- const Eigen::Matrix<double, number_of_states, number_of_inputs> B_continuous;
const Eigen::Matrix<double, number_of_outputs, number_of_states> C;
const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D;
const Eigen::Matrix<double, number_of_inputs, 1> U_min;
@@ -178,7 +165,7 @@
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) {
+ const Eigen::Matrix<double, number_of_inputs, 1> &U) const {
return A() * X + B() * U;
}
@@ -216,6 +203,202 @@
};
template <int number_of_states, int number_of_inputs, int number_of_outputs>
+struct StateFeedbackHybridPlantCoefficients final {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ StateFeedbackHybridPlantCoefficients(
+ const StateFeedbackHybridPlantCoefficients &other)
+ : A_continuous(other.A_continuous),
+ B_continuous(other.B_continuous),
+ C(other.C),
+ D(other.D),
+ U_min(other.U_min),
+ U_max(other.U_max) {}
+
+ StateFeedbackHybridPlantCoefficients(
+ const Eigen::Matrix<double, number_of_states, number_of_states>
+ &A_continuous,
+ const Eigen::Matrix<double, number_of_states, number_of_inputs>
+ &B_continuous,
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &C,
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_max,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_min)
+ : A_continuous(A_continuous),
+ B_continuous(B_continuous),
+ C(C),
+ D(D),
+ U_min(U_min),
+ U_max(U_max) {}
+
+ const Eigen::Matrix<double, number_of_states, number_of_states> A_continuous;
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> B_continuous;
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> C;
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> D;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_min;
+ const Eigen::Matrix<double, number_of_inputs, 1> U_max;
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class StateFeedbackHybridPlant {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ StateFeedbackHybridPlant(
+ ::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ *coefficients)
+ : coefficients_(::std::move(*coefficients)), index_(0) {
+ Reset();
+ }
+
+ StateFeedbackHybridPlant(StateFeedbackHybridPlant &&other)
+ : index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ X_.swap(other.X_);
+ Y_.swap(other.Y_);
+ }
+
+ virtual ~StateFeedbackHybridPlant() {}
+
+ const Eigen::Matrix<double, number_of_states, number_of_states> &A() const {
+ return A_;
+ }
+ double A(int i, int j) const { return A()(i, j); }
+ const Eigen::Matrix<double, number_of_states, number_of_inputs> &B() const {
+ return B_;
+ }
+ double B(int i, int j) const { return B()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_states> &C() const {
+ return coefficients().C;
+ }
+ double C(int i, int j) const { return C()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, number_of_inputs> &D() const {
+ return coefficients().D;
+ }
+ double D(int i, int j) const { return D()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_min() const {
+ return coefficients().U_min;
+ }
+ double U_min(int i, int j) const { return U_min()(i, j); }
+ const Eigen::Matrix<double, number_of_inputs, 1> &U_max() const {
+ return coefficients().U_max;
+ }
+ double U_max(int i, int j) const { return U_max()(i, j); }
+
+ const Eigen::Matrix<double, number_of_states, 1> &X() const { return X_; }
+ double X(int i, int j) const { return X()(i, j); }
+ const Eigen::Matrix<double, number_of_outputs, 1> &Y() const { return Y_; }
+ double Y(int i, int j) const { return Y()(i, j); }
+
+ Eigen::Matrix<double, number_of_states, 1> &mutable_X() { return X_; }
+ double &mutable_X(int i, int j) { return mutable_X()(i, j); }
+ Eigen::Matrix<double, number_of_outputs, 1> &mutable_Y() { return Y_; }
+ double &mutable_Y(int i, int j) { return mutable_Y()(i, j); }
+
+ const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const StateFeedbackHybridPlantCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ int index() const { return index_; }
+ void set_index(int index) {
+ assert(index >= 0);
+ assert(index < static_cast<int>(coefficients_.size()));
+ index_ = index;
+ }
+
+ void Reset() {
+ X_.setZero();
+ Y_.setZero();
+ A_.setZero();
+ B_.setZero();
+ UpdateAB(::aos::controls::kLoopFrequency);
+ }
+
+ // Assert that U is within the hardware range.
+ virtual void CheckU(const Eigen::Matrix<double, number_of_inputs, 1> &U) {
+ for (int i = 0; i < kNumInputs; ++i) {
+ if (U(i, 0) > U_max(i, 0) + 0.00001 || U(i, 0) < U_min(i, 0) - 0.00001) {
+ LOG(FATAL, "U out of range\n");
+ }
+ }
+ }
+
+ // Computes the new X and Y given the control input.
+ void Update(const Eigen::Matrix<double, number_of_inputs, 1> &U,
+ ::std::chrono::nanoseconds dt) {
+ // 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;
+ }
+
+ 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,
+ ::std::chrono::nanoseconds dt) {
+ UpdateAB(dt);
+ return A() * X + B() * U;
+ }
+
+ protected:
+ // these are accessible from non-templated subclasses
+ static const int kNumStates = number_of_states;
+ static const int kNumOutputs = number_of_outputs;
+ static const int kNumInputs = number_of_inputs;
+
+ private:
+ void UpdateAB(::std::chrono::nanoseconds dt) {
+ Eigen::Matrix<double, number_of_states + number_of_inputs,
+ number_of_states + number_of_inputs>
+ M_state_continuous;
+ M_state_continuous.setZero();
+ M_state_continuous.template block<number_of_states, number_of_states>(0,
+ 0) =
+ coefficients().A_continuous *
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ .count();
+ M_state_continuous.template block<number_of_states, number_of_inputs>(
+ 0, number_of_states) =
+ coefficients().B_continuous *
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ .count();
+
+ Eigen::Matrix<double, number_of_states + number_of_inputs,
+ number_of_states + number_of_inputs>
+ M_state = M_state_continuous.exp();
+ A_ = M_state.template block<number_of_states, number_of_states>(0, 0);
+ B_ = M_state.template block<number_of_states, number_of_inputs>(
+ 0, number_of_states);
+ }
+
+ Eigen::Matrix<double, number_of_states, 1> X_;
+ Eigen::Matrix<double, number_of_outputs, 1> Y_;
+
+ Eigen::Matrix<double, number_of_states, number_of_states> A_;
+ Eigen::Matrix<double, number_of_states, number_of_inputs> B_;
+
+
+ ::std::vector<::std::unique_ptr<StateFeedbackHybridPlantCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
+
+ int index_;
+
+ DISALLOW_COPY_AND_ASSIGN(StateFeedbackHybridPlant);
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
class StateFeedbackController {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
@@ -238,10 +421,6 @@
return coefficients().Kff;
}
double Kff(int i, int j) const { return Kff()(i, j); }
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
- return coefficients().L;
- }
- double L(int i, int j) const { return L()(i, j); }
void Reset() {}
@@ -277,7 +456,6 @@
coefficients_;
};
-
// A container for all the observer coefficients.
template <int number_of_states, int number_of_inputs, int number_of_outputs>
struct StateFeedbackObserverCoefficients final {
@@ -315,15 +493,22 @@
}
Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
- void Reset() { X_hat_.setZero(); }
+ void Reset(
+ StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ StateFeedbackObserver> * /*loop*/) {
+ X_hat_.setZero();
+ }
- void Predict(const StateFeedbackLoop<
- number_of_states, number_of_inputs, number_of_outputs,
- StateFeedbackPlant<number_of_states, number_of_inputs,
- number_of_outputs>,
- StateFeedbackObserver> &loop,
- const Eigen::Matrix<double, number_of_inputs, 1> &new_u) {
- mutable_X_hat() = loop.plant().A() * X_hat() + loop.plant().B() * new_u;
+ void Predict(
+ StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ StateFeedbackObserver> *loop,
+ const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ ::std::chrono::nanoseconds /*dt*/) {
+ mutable_X_hat() = loop->plant().Update(X_hat(), new_u);
}
void Correct(const StateFeedbackLoop<
@@ -372,6 +557,199 @@
coefficients_;
};
+// A container for all the observer coefficients.
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+struct HybridKalmanCoefficients final {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ const Eigen::Matrix<double, number_of_states, number_of_states> Q_continuous;
+ const Eigen::Matrix<double, number_of_outputs, number_of_outputs> R_continuous;
+ const Eigen::Matrix<double, number_of_states, number_of_states> P_steady_state;
+
+ HybridKalmanCoefficients(
+ const Eigen::Matrix<double, number_of_states, number_of_states>
+ &Q_continuous,
+ const Eigen::Matrix<double, number_of_outputs, number_of_outputs>
+ &R_continuous,
+ const Eigen::Matrix<double, number_of_states, number_of_states>
+ &P_steady_state)
+ : Q_continuous(Q_continuous),
+ R_continuous(R_continuous),
+ P_steady_state(P_steady_state) {}
+};
+
+template <int number_of_states, int number_of_inputs, int number_of_outputs>
+class HybridKalman {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+ explicit HybridKalman(
+ ::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>> *observers)
+ : coefficients_(::std::move(*observers)) {}
+
+ HybridKalman(HybridKalman &&other)
+ : X_hat_(other.X_hat_), index_(other.index_) {
+ ::std::swap(coefficients_, other.coefficients_);
+ }
+
+ // Getters for Q
+ const Eigen::Matrix<double, number_of_states, number_of_states> &Q() const {
+ return Q_;
+ }
+ double Q(int i, int j) const { return Q()(i, j); }
+ // Getters for R
+ const Eigen::Matrix<double, number_of_outputs, number_of_outputs> &R() const {
+ return R_;
+ }
+ double R(int i, int j) const { return R()(i, j); }
+
+ // Getters for P
+ const Eigen::Matrix<double, number_of_states, number_of_states> &P() const {
+ return P_;
+ }
+ double P(int i, int j) const { return P()(i, j); }
+
+ // Getters for X_hat
+ const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
+ return X_hat_;
+ }
+ Eigen::Matrix<double, number_of_states, 1> &mutable_X_hat() { return X_hat_; }
+
+ void Reset(StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ HybridKalman> *loop) {
+ X_hat_.setZero();
+ P_ = coefficients().P_steady_state;
+ UpdateQR(loop, ::aos::controls::kLoopFrequency);
+ }
+
+ void Predict(StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ HybridKalman> *loop,
+ const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ ::std::chrono::nanoseconds dt) {
+ // Trigger the predict step. This will update A() and B() in the plant.
+ mutable_X_hat() = loop->mutable_plant()->Update(X_hat(), new_u, dt);
+
+ UpdateQR(loop, dt);
+ P_ = loop->plant().A() * P_ * loop->plant().A().transpose() + Q_;
+ }
+
+ void Correct(const StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ HybridKalman> &loop,
+ const Eigen::Matrix<double, number_of_inputs, 1> &U,
+ const Eigen::Matrix<double, number_of_outputs, 1> &Y) {
+ Eigen::Matrix<double, number_of_outputs, 1> Y_bar =
+ Y - (loop.plant().C() * X_hat_ + loop.plant().D() * U);
+ Eigen::Matrix<double, number_of_outputs, number_of_outputs> S =
+ loop.plant().C() * P_ * loop.plant().C().transpose() + R_;
+ Eigen::Matrix<double, number_of_states, number_of_outputs> KalmanGain;
+ KalmanGain = (S.transpose().ldlt().solve(
+ (P() * loop.plant().C().transpose()).transpose()))
+ .transpose();
+ X_hat_ = X_hat_ + KalmanGain * Y_bar;
+ P_ = (loop.plant().coefficients().A_continuous.Identity() -
+ KalmanGain * loop.plant().C()) *
+ P();
+ }
+
+ // Sets the current controller to be index, clamped to be within range.
+ void set_index(int index) {
+ if (index < 0) {
+ index_ = 0;
+ } else if (index >= static_cast<int>(coefficients_.size())) {
+ index_ = static_cast<int>(coefficients_.size()) - 1;
+ } else {
+ index_ = index;
+ }
+ }
+
+ int index() const { return index_; }
+
+ const HybridKalmanCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients(int index) const {
+ return *coefficients_[index];
+ }
+
+ const HybridKalmanCoefficients<number_of_states, number_of_inputs,
+ number_of_outputs>
+ &coefficients() const {
+ return *coefficients_[index_];
+ }
+
+ private:
+ void UpdateQR(StateFeedbackLoop<
+ number_of_states, number_of_inputs, number_of_outputs,
+ StateFeedbackHybridPlant<number_of_states, number_of_inputs,
+ number_of_outputs>,
+ HybridKalman> *loop,
+ ::std::chrono::nanoseconds dt) {
+ // Now, compute the discrete time Q and R coefficients.
+ Eigen::Matrix<double, number_of_states, number_of_states> Qtemp =
+ (coefficients().Q_continuous +
+ coefficients().Q_continuous.transpose()) /
+ 2.0;
+ Eigen::Matrix<double, number_of_outputs, number_of_outputs> Rtemp =
+ (coefficients().R_continuous +
+ coefficients().R_continuous.transpose()) /
+ 2.0;
+
+ Eigen::Matrix<double, 2 * number_of_states, 2 * number_of_states> M_gain;
+ M_gain.setZero();
+ // Set up the matrix M = [[-A, Q], [0, A.T]]
+ M_gain.template block<number_of_states, number_of_states>(0, 0) =
+ -loop->plant().coefficients().A_continuous;
+ M_gain.template block<number_of_states, number_of_states>(
+ 0, number_of_states) = Qtemp;
+ M_gain.template block<number_of_states, number_of_states>(
+ number_of_states, number_of_states) =
+ loop->plant().coefficients().A_continuous.transpose();
+
+ Eigen::Matrix<double, 2 * number_of_states, 2 *number_of_states> phi =
+ (M_gain *
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ .count())
+ .exp();
+
+ // Phi12 = phi[0:number_of_states, number_of_states:2*number_of_states]
+ // Phi22 = phi[number_of_states:2*number_of_states,
+ // number_of_states:2*number_of_states]
+ Eigen::Matrix<double, number_of_states, number_of_states> phi12 =
+ phi.block(0, number_of_states, number_of_states, number_of_states);
+ Eigen::Matrix<double, number_of_states, number_of_states> phi22 = phi.block(
+ number_of_states, number_of_states, number_of_states, number_of_states);
+
+ Q_ = phi22.transpose() * phi12;
+ Q_ = (Q_ + Q_.transpose()) / 2.0;
+ R_ = Rtemp /
+ ::std::chrono::duration_cast<::std::chrono::duration<double>>(dt)
+ .count();
+ }
+
+ // Internal state estimate.
+ Eigen::Matrix<double, number_of_states, 1> X_hat_;
+ // Internal covariance estimate.
+ Eigen::Matrix<double, number_of_states, number_of_states> P_;
+
+ // Discretized Q and R for the kalman filter.
+ Eigen::Matrix<double, number_of_states, number_of_states> Q_;
+ Eigen::Matrix<double, number_of_outputs, number_of_outputs> R_;
+
+ int index_ = 0;
+ ::std::vector<::std::unique_ptr<HybridKalmanCoefficients<
+ number_of_states, number_of_inputs, number_of_outputs>>>
+ coefficients_;
+};
+
template <int number_of_states, int number_of_inputs, int number_of_outputs,
typename PlantType = StateFeedbackPlant<
number_of_states, number_of_inputs, number_of_outputs>,
@@ -405,11 +783,6 @@
virtual ~StateFeedbackLoop() {}
- const Eigen::Matrix<double, number_of_states, number_of_outputs> &L() const {
- return observer().L();
- }
- double L(int i, int j) const { return L()(i, j); }
-
const Eigen::Matrix<double, number_of_states, 1> &X_hat() const {
return observer().X_hat();
}
@@ -451,6 +824,7 @@
}
const PlantType &plant() const { return plant_; }
+ PlantType *mutable_plant() { return &plant_; }
const StateFeedbackController<number_of_states, number_of_inputs,
number_of_outputs>
@@ -469,7 +843,7 @@
plant_.Reset();
controller_.Reset();
- observer_.Reset();
+ observer_.Reset(this);
}
// If U is outside the hardware range, limit it before the plant tries to use
@@ -507,7 +881,8 @@
}
// stop_motors is whether or not to output all 0s.
- void Update(bool stop_motors) {
+ void Update(bool stop_motors,
+ ::std::chrono::nanoseconds dt = ::std::chrono::milliseconds(5)) {
if (stop_motors) {
U_.setZero();
U_uncapped_.setZero();
@@ -517,7 +892,7 @@
CapU();
}
- UpdateObserver(U_);
+ UpdateObserver(U_, dt);
UpdateFFReference();
}
@@ -530,8 +905,9 @@
}
}
- void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u) {
- observer_.Predict(*this, new_u);
+ void UpdateObserver(const Eigen::Matrix<double, number_of_inputs, 1> &new_u,
+ ::std::chrono::nanoseconds dt) {
+ observer_.Predict(this, new_u, dt);
}
// Sets the current controller to be index.
@@ -569,8 +945,6 @@
// Computed output before being capped.
Eigen::Matrix<double, number_of_inputs, 1> U_uncapped_;
- int index_;
-
DISALLOW_COPY_AND_ASSIGN(StateFeedbackLoop);
};
diff --git a/frc971/control_loops/state_feedback_loop_test.cc b/frc971/control_loops/state_feedback_loop_test.cc
index ebe2871..072d044 100644
--- a/frc971/control_loops/state_feedback_loop_test.cc
+++ b/frc971/control_loops/state_feedback_loop_test.cc
@@ -6,6 +6,114 @@
namespace control_loops {
namespace testing {
+StateFeedbackHybridPlantCoefficients<3, 1, 1>
+MakeIntegralShooterPlantCoefficients() {
+ Eigen::Matrix<double, 1, 3> C;
+ C(0, 0) = 1.0;
+ C(0, 1) = 0.0;
+ C(0, 2) = 0.0;
+ Eigen::Matrix<double, 1, 1> D;
+ D(0, 0) = 0;
+ Eigen::Matrix<double, 1, 1> U_max;
+ U_max(0, 0) = 12.0;
+ Eigen::Matrix<double, 1, 1> U_min;
+ U_min(0, 0) = -12.0;
+ Eigen::Matrix<double, 3, 3> A_continuous;
+ A_continuous(0, 0) = 0.0;
+ A_continuous(0, 1) = 1.0;
+ A_continuous(0, 2) = 0.0;
+ A_continuous(1, 0) = 0.0;
+ A_continuous(1, 1) = -8.1021414789556374;
+ A_continuous(1, 2) = 443.75;
+ A_continuous(2, 0) = 0.0;
+ A_continuous(2, 1) = 0.0;
+ A_continuous(2, 2) = 0.0;
+ Eigen::Matrix<double, 3, 1> B_continuous;
+ B_continuous(0, 0) = 0.0;
+ 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);
+}
+
+StateFeedbackControllerCoefficients<3, 1, 1>
+MakeIntegralShooterControllerCoefficients() {
+ Eigen::Matrix<double, 1, 3> K;
+ K(0, 0) = 0.0;
+ K(0, 1) = 0.027731156542808996;
+ K(0, 2) = 1.0;
+ Eigen::Matrix<double, 1, 3> Kff;
+ Kff(0, 0) = 0.0;
+ Kff(0, 1) = 0.45989503537638587;
+ Kff(0, 2) = 0.0;
+ return StateFeedbackControllerCoefficients<3, 1, 1>(K, Kff);
+}
+
+HybridKalmanCoefficients<3, 1, 1> MakeIntegralShooterObserverCoefficients() {
+ Eigen::Matrix<double, 3, 3> Q_continuous;
+ Q_continuous(0, 0) = 0.0001;
+ Q_continuous(0, 1) = 0.0;
+ Q_continuous(0, 2) = 0.0;
+ Q_continuous(1, 0) = 0.0;
+ Q_continuous(1, 1) = 4.0;
+ Q_continuous(1, 2) = 0.0;
+ Q_continuous(2, 0) = 0.0;
+ Q_continuous(2, 1) = 0.0;
+ Q_continuous(2, 2) = 0.040000000000000008;
+ Eigen::Matrix<double, 1, 1> R_continuous;
+ R_continuous(0, 0) = 9.9999999999999995e-07;
+ Eigen::Matrix<double, 3, 3> P_steady_state;
+ P_steady_state(0, 0) = 7.1645559451160497e-05;
+ P_steady_state(0, 1) = 0.0031205034236441768;
+ P_steady_state(0, 2) = 0.00016022137220036598;
+ P_steady_state(1, 0) = 0.0031205034236441768;
+ P_steady_state(1, 1) = 0.25313549121689616;
+ P_steady_state(1, 2) = 0.015962850974712596;
+ P_steady_state(2, 0) = 0.00016022137220036598;
+ 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);
+}
+
+StateFeedbackHybridPlant<3, 1, 1> MakeIntegralShooterPlant() {
+ ::std::vector<
+ ::std::unique_ptr<StateFeedbackHybridPlantCoefficients<3, 1, 1>>>
+ plants(1);
+ plants[0] = ::std::unique_ptr<StateFeedbackHybridPlantCoefficients<3, 1, 1>>(
+ new StateFeedbackHybridPlantCoefficients<3, 1, 1>(
+ MakeIntegralShooterPlantCoefficients()));
+ return StateFeedbackHybridPlant<3, 1, 1>(&plants);
+}
+
+StateFeedbackController<3, 1, 1> MakeIntegralShooterController() {
+ ::std::vector<::std::unique_ptr<StateFeedbackControllerCoefficients<3, 1, 1>>>
+ controllers(1);
+ controllers[0] =
+ ::std::unique_ptr<StateFeedbackControllerCoefficients<3, 1, 1>>(
+ new StateFeedbackControllerCoefficients<3, 1, 1>(
+ MakeIntegralShooterControllerCoefficients()));
+ return StateFeedbackController<3, 1, 1>(&controllers);
+}
+
+HybridKalman<3, 1, 1> MakeIntegralShooterObserver() {
+ ::std::vector<::std::unique_ptr<HybridKalmanCoefficients<3, 1, 1>>> observers(
+ 1);
+ observers[0] = ::std::unique_ptr<HybridKalmanCoefficients<3, 1, 1>>(
+ new HybridKalmanCoefficients<3, 1, 1>(
+ MakeIntegralShooterObserverCoefficients()));
+ return HybridKalman<3, 1, 1>(&observers);
+}
+
+StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>
+MakeIntegralShooterLoop() {
+ return StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>(
+ MakeIntegralShooterPlant(), MakeIntegralShooterController(),
+ MakeIntegralShooterObserver());
+}
+
// Tests that everything compiles and nothing crashes even if
// number_of_inputs!=number_of_outputs.
// There used to be lots of bugs in this area.
@@ -15,8 +123,6 @@
const StateFeedbackPlantCoefficients<2, 4, 7> coefficients(
Eigen::Matrix<double, 2, 2>::Identity(),
Eigen::Matrix<double, 2, 2>::Identity(),
- Eigen::Matrix<double, 2, 2>::Identity(),
- Eigen::Matrix<double, 2, 4>::Identity(),
Eigen::Matrix<double, 2, 4>::Identity(),
Eigen::Matrix<double, 7, 2>::Identity(),
Eigen::Matrix<double, 7, 4>::Identity(),
@@ -54,6 +160,32 @@
test_loop.CapU();
}
+// Tests that the continuous to discrete calculation for the kalman filter
+// matches what was computed both in Python and in Matlab.
+TEST(StateFeedbackLoopTest, PythonMatch) {
+ auto test_loop = MakeIntegralShooterLoop();
+ test_loop.Update(false, ::std::chrono::milliseconds(5));
+
+ Eigen::Matrix<double, 3, 3> A_discrete;
+ A_discrete << 1, 0.00490008, 0.00547272, 0, 0.96029888, 2.17440921, 0, 0, 1;
+
+ Eigen::Matrix<double, 3, 1> B_discrete;
+ B_discrete << 0.00547272, 2.17440921, 0;
+
+ Eigen::Matrix<double, 3, 3> Q_discrete;
+ Q_discrete << 6.62900602e-07, 4.86205253e-05, 3.66076676e-07, 4.86205253e-05,
+ 1.95296358e-02, 2.18908995e-04, 3.66076676e-07, 2.18908995e-04,
+ 2.00000000e-04;
+
+ Eigen::Matrix<double, 1, 1> R_discrete;
+ R_discrete << 0.0002;
+
+ EXPECT_TRUE(A_discrete.isApprox(test_loop.plant().A(), 0.001));
+ EXPECT_TRUE(B_discrete.isApprox(test_loop.plant().B(), 0.001));
+ EXPECT_TRUE(Q_discrete.isApprox(test_loop.observer().Q(), 0.001));
+ EXPECT_TRUE(R_discrete.isApprox(test_loop.observer().R(), 0.001));
+}
+
} // namespace testing
} // namespace control_loops
} // namespace frc971
diff --git a/y2015/control_loops/python/polydrivetrain.py b/y2015/control_loops/python/polydrivetrain.py
index 29a55a6..b601853 100755
--- a/y2015/control_loops/python/polydrivetrain.py
+++ b/y2015/control_loops/python/polydrivetrain.py
@@ -112,7 +112,7 @@
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
right_low=right_low)
- self.dt = 0.01
+ self.dt = 0.005
self.A_continuous = numpy.matrix(
[[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
[self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
@@ -129,7 +129,7 @@
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.PlaceControllerPoles([0.6, 0.6])
+ self.PlaceControllerPoles([0.7, 0.7])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high
@@ -178,7 +178,7 @@
[[-12.0000000000],
[-12.0000000000]])
- self.dt = 0.01
+ self.dt = 0.005
self.R = numpy.matrix(
[[0.0],
diff --git a/y2015_bot3/control_loops/python/polydrivetrain.py b/y2015_bot3/control_loops/python/polydrivetrain.py
index 1d2a74e..8efe374 100755
--- a/y2015_bot3/control_loops/python/polydrivetrain.py
+++ b/y2015_bot3/control_loops/python/polydrivetrain.py
@@ -112,7 +112,7 @@
super(VelocityDrivetrainModel, self).__init__(name)
self._drivetrain = drivetrain.Drivetrain(left_low=left_low,
right_low=right_low)
- self.dt = 0.01
+ self.dt = 0.005
self.A_continuous = numpy.matrix(
[[self._drivetrain.A_continuous[1, 1], self._drivetrain.A_continuous[1, 3]],
[self._drivetrain.A_continuous[3, 1], self._drivetrain.A_continuous[3, 3]]])
@@ -129,7 +129,7 @@
# FF * X = U (steady state)
self.FF = self.B.I * (numpy.eye(2) - self.A)
- self.PlaceControllerPoles([0.6, 0.6])
+ self.PlaceControllerPoles([0.7, 0.7])
self.PlaceObserverPoles([0.02, 0.02])
self.G_high = self._drivetrain.G_high
@@ -178,7 +178,7 @@
[[-12.0000000000],
[-12.0000000000]])
- self.dt = 0.01
+ self.dt = 0.005
self.R = numpy.matrix(
[[0.0],
diff --git a/y2017/control_loops/python/shooter.py b/y2017/control_loops/python/shooter.py
index 6c0f2f3..0858e45 100755
--- a/y2017/control_loops/python/shooter.py
+++ b/y2017/control_loops/python/shooter.py
@@ -14,7 +14,14 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-class VelocityShooter(control_loop.ControlLoop):
+
+def PlotDiff(list1, list2, time):
+ pylab.subplot(1, 1, 1)
+ pylab.plot(time, numpy.subtract(list1, list2), label='diff')
+ pylab.legend()
+
+
+class VelocityShooter(control_loop.HybridControlLoop):
def __init__(self, name='VelocityShooter'):
super(VelocityShooter, self).__init__(name)
# Number of motors
@@ -141,14 +148,19 @@
q_pos = 0.01
q_vel = 2.0
q_voltage = 0.2
- self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
- [0.0, (q_vel ** 2.0), 0.0],
- [0.0, 0.0, (q_voltage ** 2.0)]])
+ self.Q_continuous = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0],
+ [0.0, (q_vel ** 2.0), 0.0],
+ [0.0, 0.0, (q_voltage ** 2.0)]])
r_pos = 0.001
- self.R = numpy.matrix([[(r_pos ** 2.0)]])
+ self.R_continuous = numpy.matrix([[(r_pos ** 2.0)]])
- self.KalmanGain, self.Q_steady = controls.kalman(
+ _, _, self.Q, self.R = controls.kalmd(
+ A_continuous=self.A_continuous, B_continuous=self.B_continuous,
+ Q_continuous=self.Q_continuous, R_continuous=self.R_continuous,
+ dt=self.dt)
+
+ self.KalmanGain, self.P_steady_state = controls.kalman(
A=self.A, B=self.B, C=self.C, Q=self.Q, R=self.R)
self.L = self.A * self.KalmanGain
@@ -173,9 +185,10 @@
self.x_hat = []
self.u = []
self.offset = []
+ self.diff = []
def run_test(self, shooter, goal, iterations=200, controller_shooter=None,
- observer_shooter=None):
+ observer_shooter=None, hybrid_obs = False):
"""Runs the shooter plant with an initial condition and goal.
Args:
@@ -211,6 +224,7 @@
U[0, 0] = numpy.clip(U[0, 0], -vbat, vbat)
self.x.append(shooter.X[0, 0])
+ self.diff.append(shooter.X[1, 0] - observer_shooter.X_hat[1, 0])
if self.v:
last_v = self.v[-1]
@@ -221,8 +235,9 @@
self.a.append((self.v[-1] - last_v) / shooter.dt)
if observer_shooter is not None:
- observer_shooter.Y = shooter.Y
- observer_shooter.CorrectObserver(U)
+ if i != 0:
+ observer_shooter.Y = shooter.Y
+ observer_shooter.CorrectObserver(U)
self.offset.append(observer_shooter.X_hat[2, 0])
applied_U = U.copy()
@@ -231,7 +246,11 @@
shooter.Update(applied_U)
if observer_shooter is not None:
- observer_shooter.PredictObserver(U)
+ if hybrid_obs:
+ observer_shooter.PredictHybridObserver(U, shooter.dt)
+ else:
+ observer_shooter.PredictObserver(U)
+
self.t.append(initial_t + i * shooter.dt)
self.u.append(U[0, 0])
@@ -251,24 +270,42 @@
pylab.plot(self.t, self.a, label='a')
pylab.legend()
- pylab.show()
+ pylab.figure()
+ pylab.draw()
def main(argv):
scenario_plotter = ScenarioPlotter()
- shooter = Shooter()
- shooter_controller = IntegralShooter()
- observer_shooter = IntegralShooter()
-
- initial_X = numpy.matrix([[0.0], [0.0]])
- R = numpy.matrix([[0.0], [100.0], [0.0]])
- scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
- observer_shooter=observer_shooter, iterations=200)
-
if FLAGS.plot:
+ shooter = Shooter()
+ shooter_controller = IntegralShooter()
+ observer_shooter = IntegralShooter()
+ iterations = 200
+
+ initial_X = numpy.matrix([[0.0], [0.0]])
+ R = numpy.matrix([[0.0], [100.0], [0.0]])
+ scenario_plotter.run_test(shooter, goal=R, controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter, iterations=iterations)
+
scenario_plotter.Plot()
+ scenario_plotter_int = ScenarioPlotter()
+
+ shooter = Shooter()
+ shooter_controller = IntegralShooter()
+ observer_shooter_hybrid = IntegralShooter()
+
+ scenario_plotter_int.run_test(shooter, goal=R, controller_shooter=shooter_controller,
+ observer_shooter=observer_shooter_hybrid, iterations=iterations,
+ hybrid_obs = True)
+
+ scenario_plotter_int.Plot()
+ PlotDiff(scenario_plotter.x_hat, scenario_plotter_int.x_hat,
+ scenario_plotter.t)
+
+ pylab.show()
+
if len(argv) != 5:
glog.fatal('Expected .h file name and .cc file name')
else:
@@ -284,7 +321,9 @@
integral_shooter = IntegralShooter('IntegralShooter')
integral_loop_writer = control_loop.ControlLoopWriter(
- 'IntegralShooter', [integral_shooter], namespaces=namespaces)
+ 'IntegralShooter', [integral_shooter], namespaces=namespaces,
+ plant_type='StateFeedbackHybridPlant',
+ observer_type='HybridKalman')
integral_loop_writer.Write(argv[3], argv[4])
diff --git a/y2017/control_loops/superstructure/indexer/indexer.cc b/y2017/control_loops/superstructure/indexer/indexer.cc
index 6186f7f..0560ec4 100644
--- a/y2017/control_loops/superstructure/indexer/indexer.cc
+++ b/y2017/control_loops/superstructure/indexer/indexer.cc
@@ -116,7 +116,8 @@
position_error_ = X_hat_current_(0, 0) - Y_(0, 0);
loop_->Update(disabled);
- stuck_indexer_detector_->UpdateObserver(loop_->U());
+ stuck_indexer_detector_->UpdateObserver(loop_->U(),
+ ::aos::controls::kLoopFrequency);
}
void IndexerController::SetStatus(IndexerStatus *status) {
diff --git a/y2017/control_loops/superstructure/shooter/shooter.cc b/y2017/control_loops/superstructure/shooter/shooter.cc
index c4d1630..8991658 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.cc
+++ b/y2017/control_loops/superstructure/shooter/shooter.cc
@@ -23,7 +23,8 @@
// TODO(austin): Pseudo current limit?
ShooterController::ShooterController()
- : loop_(new StateFeedbackLoop<3, 1, 1>(
+ : loop_(new StateFeedbackLoop<3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>,
+ HybridKalman<3, 1, 1>>(
superstructure::shooter::MakeIntegralShooterLoop())) {
history_.fill(0);
Y_.setZero();
diff --git a/y2017/control_loops/superstructure/shooter/shooter.h b/y2017/control_loops/superstructure/shooter/shooter.h
index 41e24c0..7dc44bf 100644
--- a/y2017/control_loops/superstructure/shooter/shooter.h
+++ b/y2017/control_loops/superstructure/shooter/shooter.h
@@ -49,7 +49,9 @@
// The current sensor measurement.
Eigen::Matrix<double, 1, 1> Y_;
// The control loop.
- ::std::unique_ptr<StateFeedbackLoop<3, 1, 1>> loop_;
+ ::std::unique_ptr<StateFeedbackLoop<
+ 3, 1, 1, StateFeedbackHybridPlant<3, 1, 1>, HybridKalman<3, 1, 1>>>
+ loop_;
// History array for calculating a filtered angular velocity.
static constexpr int kHistoryLength = 5;