commit 7dedecc2987220d53b14977665456c195dd5db30
author James Kuszmaul <jabukuszmaul@gmail.com> Wed May 01 21:45:17 2019 -0500
committer James Kuszmaul <jabukuszmaul@gmail.com> Wed May 01 20:10:50 2019 -0700
tree 02acc3625cdb9f862320a7a36dde0c6fc297277e
parent f03dddfede1cfa96f3d4746698ec6d801b9f9e16

Tune Kalman filter initial covariance

Change-Id: I46bd201f892ef8609496a7d9e76e02d9e551cc04

frc971/control_loops/drivetrain/hybrid_ekf.h

diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index ae92069..5cecce7 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -469,7 +469,7 @@
   Q_continuous_(kAngularError, kAngularError) = ::std::pow(2.0, 2.0);
 
   P_.setZero();
-  P_.diagonal() << 0.1, 0.1, 0.01, 0.02, 0.01, 0.02, 0.01, 1, 1, 0.03;
+  P_.diagonal() << 0.01, 0.01, 0.01, 0.02, 0.01, 0.02, 0.01, 1, 1, 0.03;
 
   H_encoders_and_gyro_.setZero();
   // Encoders are stored directly in the state matrix, so are a minor