Tuned drivetrain kalman filter.
Change-Id: I630770a1331cf896bc6d2de6a58fee880c0185c3
diff --git a/y2014/control_loops/drivetrain/drivetrain.cc b/y2014/control_loops/drivetrain/drivetrain.cc
index 00686f0..23b03f3 100644
--- a/y2014/control_loops/drivetrain/drivetrain.cc
+++ b/y2014/control_loops/drivetrain/drivetrain.cc
@@ -13,6 +13,7 @@
#include "y2014/constants.h"
#include "y2014/control_loops/drivetrain/drivetrain.q.h"
#include "y2014/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2014/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
#include "y2014/control_loops/drivetrain/polydrivetrain.h"
#include "y2014/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/queues/gyro.q.h"
@@ -28,6 +29,14 @@
namespace control_loops {
namespace drivetrain {
+DrivetrainLoop::DrivetrainLoop(
+ ::frc971::control_loops::DrivetrainQueue *my_drivetrain)
+ : aos::controls::ControlLoop<::frc971::control_loops::DrivetrainQueue>(
+ my_drivetrain),
+ kf_(::y2014::control_loops::drivetrain::MakeKFDrivetrainLoop()) {
+ ::aos::controls::HPolytope<0>::Init();
+}
+
void DrivetrainLoop::RunIteration(
const ::frc971::control_loops::DrivetrainQueue::Goal *goal,
const ::frc971::control_loops::DrivetrainQueue::Position *position,
@@ -69,6 +78,8 @@
LOG_STRUCT(DEBUG, "using", *gyro_reading.get());
dt_closedloop_.SetPosition(left_encoder, right_encoder,
gyro_reading->angle);
+ last_gyro_heading_ = gyro_reading->angle;
+ last_gyro_rate_ = gyro_reading->velocity;
} else {
dt_closedloop_.SetRawPosition(left_encoder, right_encoder);
}
@@ -110,6 +121,42 @@
status->uncapped_left_voltage = dt_closedloop_.loop().U_uncapped(0, 0);
status->uncapped_right_voltage = dt_closedloop_.loop().U_uncapped(1, 0);
}
+
+
+ double left_voltage = 0.0;
+ double right_voltage = 0.0;
+ if (output) {
+ left_voltage = output->left_voltage;
+ right_voltage = output->right_voltage;
+ }
+
+ const double scalar = ::aos::robot_state->voltage_battery / 12.0;
+
+ left_voltage *= scalar;
+ right_voltage *= scalar;
+
+ kf_.set_controller_index(dt_openloop_.controller_index());
+
+ Eigen::Matrix<double, 3, 1> Y;
+ Y << position->left_encoder, position->right_encoder, last_gyro_rate_;
+ kf_.Correct(Y);
+ integrated_kf_heading_ +=
+ kDt * (kf_.X_hat(3, 0) - kf_.X_hat(1, 0)) / (kRobotRadius * 2.0);
+
+ // To validate, look at the following:
+
+ // Observed - dx/dt velocity for left, right.
+
+ // Angular velocity error compared to the gyro
+ // Gyro heading vs left-right
+ // Voltage error.
+
+ Eigen::Matrix<double, 2, 1> U;
+ U << last_left_voltage_, last_right_voltage_;
+ last_left_voltage_ = left_voltage;
+ last_right_voltage_ = right_voltage;
+
+ kf_.UpdateObserver(U);
}
} // namespace drivetrain
diff --git a/y2014/control_loops/drivetrain/drivetrain.h b/y2014/control_loops/drivetrain/drivetrain.h
index a77ab15..5b8da12 100644
--- a/y2014/control_loops/drivetrain/drivetrain.h
+++ b/y2014/control_loops/drivetrain/drivetrain.h
@@ -22,11 +22,7 @@
// drivetrain at frc971::control_loops::drivetrain
explicit DrivetrainLoop(
::frc971::control_loops::DrivetrainQueue *my_drivetrain =
- &::frc971::control_loops::drivetrain_queue)
- : aos::controls::ControlLoop<control_loops::DrivetrainQueue>(
- my_drivetrain) {
- ::aos::controls::HPolytope<0>::Init();
- }
+ &::frc971::control_loops::drivetrain_queue);
protected:
// Executes one cycle of the control loop.
@@ -39,9 +35,17 @@
typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
SimpleLogInterval no_position_ = SimpleLogInterval(
::aos::time::Time::InSeconds(0.25), WARNING, "no position");
+ double last_gyro_heading_ = 0.0;
+ double last_gyro_rate_ = 0.0;
PolyDrivetrain dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;
+ StateFeedbackLoop<7, 2, 3> kf_;
+
+ double last_left_voltage_ = 0;
+ double last_right_voltage_ = 0;
+
+ double integrated_kf_heading_ = 0;
};
} // namespace drivetrain
diff --git a/y2014/control_loops/drivetrain/ssdrivetrain.h b/y2014/control_loops/drivetrain/ssdrivetrain.h
index f79a993..5945f96 100644
--- a/y2014/control_loops/drivetrain/ssdrivetrain.h
+++ b/y2014/control_loops/drivetrain/ssdrivetrain.h
@@ -51,6 +51,9 @@
return loop_->X_hat(0, 0);
}
+ double left_velocity() const { return loop_->X_hat(1, 0); }
+ double right_velocity() const { return loop_->X_hat(3, 0); }
+
double GetEstimatedRightEncoder() const {
return loop_->X_hat(2, 0);
}
diff --git a/y2014/control_loops/python/drivetrain.py b/y2014/control_loops/python/drivetrain.py
index 43d5a19..2a93285 100755
--- a/y2014/control_loops/python/drivetrain.py
+++ b/y2014/control_loops/python/drivetrain.py
@@ -63,20 +63,19 @@
# Number of motors per side
self.num_motors = 2
# Stall Torque in N m
- self.stall_torque = 2.42 * self.num_motors
+ self.stall_torque = 2.42 * self.num_motors * 0.60
# Stall Current in Amps
self.stall_current = 133.0 * self.num_motors
# Free Speed in RPM. Used number from last year.
- self.free_speed = 4650.0
+ self.free_speed = 5500.0
# Free Current in Amps
- self.free_current = 2.7 * self.num_motors
+ self.free_current = 4.7 * self.num_motors
# Moment of inertia of the drivetrain in kg m^2
- # Just borrowed from last year.
- self.J = 4.5
+ self.J = 2.8
# Mass of the robot, in kg.
self.m = 68
# Radius of the robot, in meters (from last year).
- self.rb = 0.617998644 / 2.0
+ self.rb = 0.647998644 / 2.0
# Radius of the wheels, in meters.
self.r = .04445
# Resistance of the motor, divided by the number of motors.
@@ -132,11 +131,6 @@
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- # Poles from last year.
- self.hp = 0.65
- self.lp = 0.83
- self.PlaceControllerPoles([self.hp, self.lp, self.hp, self.lp])
-
q_pos = 0.12
q_vel = 1.0
self.Q = numpy.matrix([[(1.0 / (q_pos ** 2.0)), 0.0, 0.0, 0.0],
@@ -179,22 +173,24 @@
self.A_continuous[0:4,0:4] = self.unaugmented_A_continuous
self.A_continuous[0:4,4:6] = self.unaugmented_B_continuous
self.B_continuous[0:4,0:2] = self.unaugmented_B_continuous
+ self.A_continuous[0,6] = 1
+ self.A_continuous[2,6] = -1
self.A, self.B = self.ContinuousToDiscrete(
self.A_continuous, self.B_continuous, self.dt)
- self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, self.rb],
- [0, 0, 1, 0, 0, 0, -self.rb],
- [0, -2.0 / self.rb, 0, 2.0 / self.rb, 0, 0, 0]])
+ self.C = numpy.matrix([[1, 0, 0, 0, 0, 0, 0],
+ [0, 0, 1, 0, 0, 0, 0],
+ [0, -0.5 / self.rb, 0, 0.5 / self.rb, 0, 0, 0]])
self.D = numpy.matrix([[0, 0],
[0, 0],
[0, 0]])
- q_pos = 0.08
- q_vel = 0.40
- q_voltage = 6.0
- q_gyro = 0.1
+ q_pos = 0.05
+ q_vel = 1.00
+ q_voltage = 10.0
+ q_encoder_uncertainty = 2.00
self.Q = numpy.matrix([[(q_pos ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
[0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0, 0.0, 0.0],
@@ -202,10 +198,10 @@
[0.0, 0.0, 0.0, (q_vel ** 2.0), 0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0, 0.0],
[0.0, 0.0, 0.0, 0.0, 0.0, (q_voltage ** 2.0), 0.0],
- [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_gyro ** 2.0)]])
+ [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, (q_encoder_uncertainty ** 2.0)]])
- r_pos = 0.05
- r_gyro = 0.001
+ r_pos = 0.0001
+ r_gyro = 0.000001
self.R = numpy.matrix([[(r_pos ** 2.0), 0.0, 0.0],
[0.0, (r_pos ** 2.0), 0.0],
[0.0, 0.0, (r_gyro ** 2.0)]])