diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp new file mode 100644 index 0000000000..6d5c5bd8fc --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp @@ -0,0 +1,157 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * This is a sample program to demonstrate how to use a state-space controller + * to control an arm. + */ +class Robot : public frc::TimedRobot { + static constexpr int kMotorPort = 0; + static constexpr int kEncoderAChannel = 0; + static constexpr int kEncoderBChannel = 1; + static constexpr int kJoystickPort = 0; + + static constexpr units::radian_t kRaisedPosition = 90_deg; + static constexpr units::radian_t kLoweredPosition = 0_deg; + + // Moment of inertia of the arm. Can be estimated with CAD. If finding this + // constant is difficult, LinearSystem.identifyPositionSystem may be better. + static constexpr units::kilogram_square_meter_t kArmMOI = 1.2_kg_sq_m; + + // Reduction between motors and encoder, as output over input. If the arm + // spins slower than the motors, this number should be greater than one. + static constexpr double kArmGearing = 10.0; + + // The plant holds a state-space model of our arm. This system has the + // following properties: + // + // States: [position, velocity], in radians and radians per second. + // Inputs (what we can "put in"): [voltage], in volts. + // Outputs (what we can measure): [position], in radians. + frc::LinearSystem<2, 1, 1> m_armPlant = + frc::LinearSystemId::SingleJointedArmSystem(frc::DCMotor::NEO(2), kArmMOI, + kArmGearing); + + // The observer fuses our encoder data and voltage inputs to reject noise. + frc::KalmanFilter<2, 1, 1> m_observer{ + m_armPlant, + {0.015, 0.17}, // How accurate we think our model is + {0.01}, // How accurate we think our encoder position + // data is. In this case we very highly trust our encoder position + // reading. + 20_ms}; + + // A LQR uses feedback to create voltage commands. + frc::LinearQuadraticRegulator<2, 1> m_controller{ + m_armPlant, + // qelms. Velocity error tolerance, in radians and radians per second. + // Decrease this to more heavily penalize state excursion, or make the + // controller behave more aggressively. + {1.0 * 2.0 * wpi::math::pi / 360.0, 10.0 * 2.0 * wpi::math::pi / 360.0}, + // rho balances Q and R. Increasing this will penalize state excursion + // more heavily, while decreasing this will penalize control effort more + // heavily. + 1.0, + // relms. Control effort (voltage) tolerance. Decrease this to more + // heavily penalize control effort, or make the controller less + // aggressive. 12 is a good starting point because that is the + // (approximate) maximum voltage of a battery. + {12.0}, + // Nominal time between loops. 20ms for TimedRobot, but can be lower if + // using notifiers. + 20_ms}; + + // Plant-inversion feedforward calculates the voltages necessary to reach our + // reference. + frc::LinearPlantInversionFeedforward<2, 1> m_feedforward{m_armPlant, 20_ms}; + + // The state-space loop combines a controller, observer, feedforward and plant + // for easy control. + frc::LinearSystemLoop<2, 1, 1> m_loop{m_armPlant, m_controller, m_feedforward, + m_observer, 12_V}; + + // An encoder set up to measure arm position in radians per second. + frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel}; + + frc::PWMVictorSPX m_motor{kMotorPort}; + frc::XboxController m_joystick{kJoystickPort}; + + frc::TrapezoidProfile::Constraints m_constraints{ + 45_deg_per_s, 90_deg_per_s / 1_s}; + + frc::TrapezoidProfile::State m_lastProfiledReference; + + public: + void RobotInit() { + // We go 2 pi radians per 4096 clicks. + m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi / 4096.0); + } + + void TeleopInit() { + m_loop.Reset( + frc::MakeMatrix<2, 1>(m_encoder.GetDistance(), m_encoder.GetRate())); + + m_lastProfiledReference = { + units::radian_t(m_encoder.GetDistance()), + units::radians_per_second_t(m_encoder.GetRate())}; + } + + void TeleopPeriodic() { + // Sets the target position of our arm. This is similar to setting the + // setpoint of a PID controller. + frc::TrapezoidProfile::State goal; + if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) { + // We pressed the bumper, so let's set our next reference + goal = {kRaisedPosition, 0_rad_per_s}; + } else { + // We released the bumper, so let's spin down + goal = {kLoweredPosition, 0_rad_per_s}; + } + m_lastProfiledReference = + (frc::TrapezoidProfile(m_constraints, goal, + m_lastProfiledReference)) + .Calculate(20_ms); + + m_loop.SetNextR( + frc::MakeMatrix<2, 1>(m_lastProfiledReference.position.to(), + m_lastProfiledReference.velocity.to())); + + // Correct our Kalman filter's state vector estimate with encoder data. + m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetDistance())); + + // Update our LQR to generate new voltage commands and use the voltages to + // predict the next state with out Kalman filter. + m_loop.Predict(20_ms); + + // Send the new calculated voltage to the motors. + // voltage = duty cycle * battery voltage, so + // duty cycle = voltage / battery voltage + m_motor.SetVoltage(units::volt_t(m_loop.U(0))); + } +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp new file mode 100644 index 0000000000..4c70fbc4f0 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp @@ -0,0 +1,156 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * This is a sample program to demonstrate how to use a state-space controller + * to control an elevator. + */ +class Robot : public frc::TimedRobot { + static constexpr int kMotorPort = 0; + static constexpr int kEncoderAChannel = 0; + static constexpr int kEncoderBChannel = 1; + static constexpr int kJoystickPort = 0; + + static constexpr units::meter_t kRaisedPosition = 2_ft; + static constexpr units::meter_t kLoweredPosition = 0_ft; + + static constexpr units::meter_t kDrumRadius = 0.75_in; + static constexpr units::kilogram_t kCarriageMass = 4.5_kg; + static constexpr double kGearRatio = 6.0; + + // The plant holds a state-space model of our elevator. This system has the + // following properties: + // + // States: [position, velocity], in meters and meters per second. + // Inputs (what we can "put in"): [voltage], in volts. + // Outputs (what we can measure): [position], in meters. + frc::LinearSystem<2, 1, 1> m_elevatorPlant = + frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), kCarriageMass, + kDrumRadius, kGearRatio); + + // The observer fuses our encoder data and voltage inputs to reject noise. + frc::KalmanFilter<2, 1, 1> m_observer{ + m_elevatorPlant, + {0.0508, 0.5}, // How accurate we think our model is + {0.001}, // How accurate we think our encoder position + // data is. In this case we very highly trust our encoder position + // reading. + 20_ms}; + + // A LQR uses feedback to create voltage commands. + frc::LinearQuadraticRegulator<2, 1> m_controller{ + m_elevatorPlant, + // qelms. State error tolerance, in meters and meters per second. + // Decrease this to more heavily penalize state excursion, or make the + // controller behave more aggressively. + {0.0254, 0.254}, + // rho balances Q and R. Increasing this will penalize state excursion + // more heavily, while decreasing this will penalize control effort more + // heavily. + 1.0, + // relms. Control effort (voltage) tolerance. Decrease this to more + // heavily penalize control effort, or make the controller less + // aggressive. 12 is a good starting point because that is the + // (approximate) maximum voltage of a battery. + {12.0}, + // Nominal time between loops. 20ms for TimedRobot, but can be lower if + // using notifiers. + 20_ms}; + + // Plant-inversion feedforward calculates the voltages necessary to reach our + // reference. + frc::LinearPlantInversionFeedforward<2, 1> m_feedforward{m_elevatorPlant, + 20_ms}; + + // The state-space loop combines a controller, observer, feedforward and plant + // for easy control. + frc::LinearSystemLoop<2, 1, 1> m_loop{m_elevatorPlant, m_controller, + m_feedforward, m_observer, 12_V}; + + // An encoder set up to measure elevator height in meters. + frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel}; + + frc::PWMVictorSPX m_motor{kMotorPort}; + frc::XboxController m_joystick{kJoystickPort}; + + frc::TrapezoidProfile::Constraints m_constraints{3_fps, + 6_fps_sq}; + + frc::TrapezoidProfile::State m_lastProfiledReference; + + public: + void RobotInit() { + // Circumference = pi * d, so distance per click = pi * d / counts + m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi * + kDrumRadius.to() / 4096.0); + } + + void TeleopInit() { + // Reset our loop to make sure it's in a known state. + m_loop.Reset( + frc::MakeMatrix<2, 1>(m_encoder.GetDistance(), m_encoder.GetRate())); + + m_lastProfiledReference = {units::meter_t(m_encoder.GetDistance()), + units::meters_per_second_t(m_encoder.GetRate())}; + } + + void TeleopPeriodic() { + // Sets the target height of our elevator. This is similar to setting the + // setpoint of a PID controller. + frc::TrapezoidProfile::State goal; + if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) { + // We pressed the bumper, so let's set our next reference + goal = {kRaisedPosition, 0_fps}; + } else { + // We released the bumper, so let's spin down + goal = {kLoweredPosition, 0_fps}; + } + m_lastProfiledReference = + (frc::TrapezoidProfile(m_constraints, goal, + m_lastProfiledReference)) + .Calculate(20_ms); + + m_loop.SetNextR( + frc::MakeMatrix<2, 1>(m_lastProfiledReference.position.to(), + m_lastProfiledReference.velocity.to())); + + // Correct our Kalman filter's state vector estimate with encoder data. + m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetDistance())); + + // Update our LQR to generate new voltage commands and use the voltages to + // predict the next state with out Kalman filter. + m_loop.Predict(20_ms); + + // Send the new calculated voltage to the motors. + // voltage = duty cycle * battery voltage, so + // duty cycle = voltage / battery voltage + m_motor.SetVoltage(units::volt_t(m_loop.U(0))); + } +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp new file mode 100644 index 0000000000..e143df8bd8 --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp @@ -0,0 +1,132 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * This is a sample program to demonstrate how to use a state-space controller + * to control a flywheel. + */ +class Robot : public frc::TimedRobot { + static constexpr int kMotorPort = 0; + static constexpr int kEncoderAChannel = 0; + static constexpr int kEncoderBChannel = 1; + static constexpr int kJoystickPort = 0; + static constexpr units::radians_per_second_t kSpinup = 500_rpm; + + static constexpr units::kilogram_square_meter_t kFlywheelMomentOfInertia = + 0.00032_kg_sq_m; + + // Reduction between motors and encoder, as output over input. If the flywheel + // spins slower than the motors, this number should be greater than one. + static constexpr double kFlywheelGearing = 1.0; + + // The plant holds a state-space model of our flywheel. This system has the + // following properties: + // + // States: [velocity], in radians per second. + // Inputs (what we can "put in"): [voltage], in volts. + // Outputs (what we can measure): [velocity], in radians per second. + frc::LinearSystem<1, 1, 1> m_flywheelPlant = + frc::LinearSystemId::FlywheelSystem( + frc::DCMotor::NEO(2), kFlywheelMomentOfInertia, kFlywheelGearing); + + // The observer fuses our encoder data and voltage inputs to reject noise. + frc::KalmanFilter<1, 1, 1> m_observer{ + m_flywheelPlant, + {3.0}, // How accurate we think our model is + {0.01}, // How accurate we think our encoder data is + 20_ms}; + + // A LQR uses feedback to create voltage commands. + frc::LinearQuadraticRegulator<1, 1> m_controller{ + m_flywheelPlant, + // qelms. Velocity error tolerance, in radians per second. Decrease this + // to more heavily penalize state excursion, or make the controller behave + // more aggressively. + {8.0}, + // rho balances Q and R, or velocity and voltage weights. Increasing this + // will penalize state excursion more heavily, while decreasing this will + // penalize control effort more heavily. + 1.0, + // relms. Control effort (voltage) tolerance. Decrease this to more + // heavily penalize control effort, or make the controller less + // aggressive. 12 is a good starting point because that is the + // (approximate) maximum voltage of a battery. + {12.0}, + // Nominal time between loops. 20ms for TimedRobot, but can be lower if + // using notifiers. + 20_ms}; + + // Plant-inversion feedforward calculates the voltages necessary to reach our + // reference. + frc::LinearPlantInversionFeedforward<1, 1> m_feedforward{m_flywheelPlant, + 20_ms}; + + // The state-space loop combines a controller, observer, feedforward and plant + // for easy control. + frc::LinearSystemLoop<1, 1, 1> m_loop{m_flywheelPlant, m_controller, + m_feedforward, m_observer, 12_V}; + + // An encoder set up to measure flywheel velocity in radians per second. + frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel}; + + frc::PWMVictorSPX m_motor{kMotorPort}; + frc::XboxController m_joystick{kJoystickPort}; + + public: + void RobotInit() { + // We go 2 pi radians per 4096 clicks. + m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi / 4096.0); + } + + void TeleopInit() { + m_loop.Reset(frc::MakeMatrix<1, 1>(m_encoder.GetRate())); + } + + void TeleopPeriodic() { + // Sets the target speed of our flywheel. This is similar to setting the + // setpoint of a PID controller. + if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) { + // We pressed the bumper, so let's set our next reference + m_loop.SetNextR(frc::MakeMatrix<1, 1>(kSpinup.to())); + } else { + // We released the bumper, so let's spin down + m_loop.SetNextR(frc::MakeMatrix<1, 1>(0.0)); + } + + // Correct our Kalman filter's state vector estimate with encoder data. + m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetRate())); + + // Update our LQR to generate new voltage commands and use the voltages to + // predict the next state with out Kalman filter. + m_loop.Predict(20_ms); + + // Send the new calculated voltage to the motors. + // voltage = duty cycle * battery voltage, so + // duty cycle = voltage / battery voltage + m_motor.SetVoltage(units::volt_t(m_loop.U(0))); + } +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp new file mode 100644 index 0000000000..34f4246a6d --- /dev/null +++ b/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp @@ -0,0 +1,132 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2017-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * This is a sample program to demonstrate how to use a state-space controller + * to control a flywheel. + */ +class Robot : public frc::TimedRobot { + static constexpr int kMotorPort = 0; + static constexpr int kEncoderAChannel = 0; + static constexpr int kEncoderBChannel = 1; + static constexpr int kJoystickPort = 0; + static constexpr units::radians_per_second_t kSpinup = 500_rpm; + + // Volts per (radian per second) + static constexpr double kFlywheelKv = 0.023; + + // Volts per (radian per second squared) + static constexpr double kFlywheelKa = 0.001; + + // The plant holds a state-space model of our flywheel. This system has the + // following properties: + // + // States: [velocity], in radians per second. + // Inputs (what we can "put in"): [voltage], in volts. + // Outputs (what we can measure): [velocity], in radians per second. + // + // The Kv and Ka constants are found using the FRC Characterization toolsuite. + frc::LinearSystem<1, 1, 1> m_flywheelPlant = + frc::LinearSystemId::IdentifyVelocitySystem(kFlywheelKv, kFlywheelKa); + + // The observer fuses our encoder data and voltage inputs to reject noise. + frc::KalmanFilter<1, 1, 1> m_observer{ + m_flywheelPlant, + {3.0}, // How accurate we think our model is + {0.01}, // How accurate we think our encoder data is + 20_ms}; + + // A LQR uses feedback to create voltage commands. + frc::LinearQuadraticRegulator<1, 1> m_controller{ + m_flywheelPlant, + // qelms. Velocity error tolerance, in radians per second. Decrease this + // to more heavily penalize state excursion, or make the controller behave + // more aggressively. + {8.0}, + // rho balances Q and R, or velocity and voltage weights. Increasing this + // will penalize state excursion more heavily, while decreasing this will + // penalize control effort more heavily. + 1.0, + // relms. Control effort (voltage) tolerance. Decrease this to more + // heavily penalize control effort, or make the controller less + // aggressive. 12 is a good starting point because that is the + // (approximate) maximum voltage of a battery. + {12.0}, + // Nominal time between loops. 20ms for TimedRobot, but can be lower if + // using notifiers. + 20_ms}; + + // Plant-inversion feedforward calculates the voltages necessary to reach our + // reference. + frc::LinearPlantInversionFeedforward<1, 1> m_feedforward{m_flywheelPlant, + 20_ms}; + + // The state-space loop combines a controller, observer, feedforward and plant + // for easy control. + frc::LinearSystemLoop<1, 1, 1> m_loop{m_flywheelPlant, m_controller, + m_feedforward, m_observer, 12_V}; + + // An encoder set up to measure flywheel velocity in radians per second. + frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel}; + + frc::PWMVictorSPX m_motor{kMotorPort}; + frc::XboxController m_joystick{kJoystickPort}; + + public: + void RobotInit() { + // We go 2 pi radians per 4096 clicks. + m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi / 4096.0); + } + + void TeleopInit() { + m_loop.Reset(frc::MakeMatrix<1, 1>(m_encoder.GetRate())); + } + + void TeleopPeriodic() { + // Sets the target speed of our flywheel. This is similar to setting the + // setpoint of a PID controller. + if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) { + // We pressed the bumper, so let's set our next reference + m_loop.SetNextR(frc::MakeMatrix<1, 1>(kSpinup.to())); + } else { + // We released the bumper, so let's spin down + m_loop.SetNextR(frc::MakeMatrix<1, 1>(0.0)); + } + + // Correct our Kalman filter's state vector estimate with encoder data. + m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetRate())); + + // Update our LQR to generate new voltage commands and use the voltages to + // predict the next state with out Kalman filter. + m_loop.Predict(20_ms); + + // Send the new calculated voltage to the motors. + // voltage = duty cycle * battery voltage, so + // duty cycle = voltage / battery voltage + m_motor.SetVoltage(units::volt_t(m_loop.U(0))); + } +}; + +#ifndef RUNNING_FRC_TESTS +int main() { return frc::StartRobot(); } +#endif diff --git a/wpilibcExamples/src/main/cpp/examples/examples.json b/wpilibcExamples/src/main/cpp/examples/examples.json index e2b74d03b5..aae80e177a 100644 --- a/wpilibcExamples/src/main/cpp/examples/examples.json +++ b/wpilibcExamples/src/main/cpp/examples/examples.json @@ -542,5 +542,74 @@ "foldername": "RamseteController", "gradlebase": "cpp", "commandversion": 2 + }, + { + "name": "StateSpaceFlywheel", + "description": "An example command-based robot demonstrating the use of a SwerveControllerCommand to follow a pregenerated trajectory.", + "tags": [ + "StateSpaceFlywheel", + "Flywheel", + "State Space", + "Model", + "Digital", + "Sensors", + "Actuators", + "Joystick" + ], + "foldername": "StateSpaceFlywheel", + "gradlebase": "cpp", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "StateSpaceFlywheelSysId", + "description": "An example state-space controller demonstrating the use of FRC Characterization's System Identification for controlling a flywheel.", + "tags": [ + "StateSpaceFlywheelSysId", + "FRC Characterization", + "Flywheel", + "Characterization", + "State space", + "Digital", + "Sensors", + "Actuators", + "Joystick" + ], + "foldername": "StateSpaceFlywheelSysId", + "gradlebase": "cpp", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "StateSpaceElevator", + "description": "An example state-space controller demonstrating the use of FRC Characterization's System Identification for controlling a flywheel.", + "tags": [ + "Elevator", + "State Space", + "Digital", + "Sensors", + "Actuators", + "Joystick" + ], + "foldername": "StateSpaceElevator", + "gradlebase": "cpp", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "StateSpaceArm", + "description": "An example state-space controller demonstrating the use of FRC Characterization's System Identification for controlling a flywheel.", + "tags": [ + "Arm", + "State space", + "Digital", + "Sensors", + "Actuators", + "Joystick" + ], + "foldername": "StateSpaceArm", + "gradlebase": "cpp", + "mainclass": "Main", + "commandversion": 2 } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json index fd5a3a1a23..47264f5bbd 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json @@ -562,5 +562,74 @@ "gradlebase": "java", "mainclass": "Main", "commandversion": 2 + }, + { + "name": "StateSpaceFlywheel", + "description": "An example state-space controller for a flywheel.", + "tags": [ + "StateSpaceFlywheel", + "Flywheel", + "State Space", + "Model", + "Digital", + "Sensors", + "Actuators", + "Joystick" + ], + "foldername": "statespaceflywheel", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "StateSpaceFlywheelSysId", + "description": "An example state-space controller for controlling a flywheel with System Identification.", + "tags": [ + "StateSpaceFlywheelSysId", + "FRC Characterization", + "Flywheel", + "Characterization", + "State space", + "Digital", + "Sensors", + "Actuators", + "Joystick" + ], + "foldername": "statespaceflywheelsysid", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "StateSpaceElevator", + "description": "An example state-space controller for controlling an elevator.", + "tags": [ + "Elevator", + "State Space", + "Digital", + "Sensors", + "Actuators", + "Joystick" + ], + "foldername": "statespaceelevator", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 + }, + { + "name": "StateSpaceArm", + "description": "An example state-space controller demonstrating the use of FRC Characterization's System Identification for controlling a flywheel.", + "tags": [ + "Arm", + "State space", + "Digital", + "Sensors", + "Actuators", + "Joystick" + ], + "foldername": "statespacearm", + "gradlebase": "java", + "mainclass": "Main", + "commandversion": 2 } ] diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java new file mode 100644 index 0000000000..c8a6fb1f5f --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespacearm; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java new file mode 100644 index 0000000000..5875567581 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java @@ -0,0 +1,153 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespacearm; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator; +import edu.wpi.first.wpilibj.estimator.KalmanFilter; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.LinearSystemLoop; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; + +/** + * This is a sample program to demonstrate how to use a state-space controller + * to control an arm. + */ +public class Robot extends TimedRobot { + private static final int kMotorPort = 0; + private static final int kEncoderAChannel = 0; + private static final int kEncoderBChannel = 1; + private static final int kJoystickPort = 0; + private static final double kRaisedPosition = Units.degreesToRadians(90.0); + private static final double kLoweredPosition = Units.degreesToRadians(0.0); + + // Moment of inertia of the arm, in kg * m^2. Can be estimated with CAD. If finding this constant + // is difficult, LinearSystem.identifyPositionSystem may be better. + private static final double kArmMOI = 1.2; + + // Reduction between motors and encoder, as output over input. If the arm spins slower than + // the motors, this number should be greater than one. + private static final double kArmGearing = 10.0; + + private final TrapezoidProfile.Constraints m_constraints = new TrapezoidProfile.Constraints( + Units.degreesToRadians(45), Units.degreesToRadians(90)); // Max arm speed and acceleration. + private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State(); + + // The plant holds a state-space model of our arm. This system has the following properties: + // + // States: [position, velocity], in radians and radians per second. + // Inputs (what we can "put in"): [voltage], in volts. + // Outputs (what we can measure): [position], in radians. + private final LinearSystem m_armPlant = + LinearSystemId.createSingleJointedArmSystem( + DCMotor.getNEO(2), + kArmMOI, + kArmGearing); + + // The observer fuses our encoder data and voltage inputs to reject noise. + private final KalmanFilter m_observer = new KalmanFilter<>( + Nat.N2(), Nat.N1(), + m_armPlant, + VecBuilder.fill(0.015, 0.17), // How accurate we + // think our model is, in radians and radians/sec + VecBuilder.fill(0.01), // How accurate we think our encoder position + // data is. In this case we very highly trust our encoder position reading. + 0.020); + + // A LQR uses feedback to create voltage commands. + private final LinearQuadraticRegulator m_controller + = new LinearQuadraticRegulator<>(m_armPlant, + VecBuilder.fill(Units.degreesToRadians(1.0), Units.degreesToRadians(10.0)), // qelms. + // Position and velocity error tolerances, in radians and radians per second. Decrease this + // to more heavily penalize state excursion, or make the controller behave more + // aggressively. In this example we weight position much more highly than velocity, but this + // can be tuned to balance the two. + 1.0, // rho balances Q and R, or velocity and voltage weights. Increasing this + // will penalize state excursion more heavily, while decreasing this will penalize control + // effort more heavily. Useful for balancing weights for systems with more states such + // as drivetrains. + VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more + // heavily penalize control effort, or make the controller less aggressive. 12 is a good + // starting point because that is the (approximate) maximum voltage of a battery. + 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be + // lower if using notifiers. + + // The state-space loop combines a controller, observer, feedforward and plant for easy control. + private final LinearSystemLoop m_loop = new LinearSystemLoop<>( + m_armPlant, + m_controller, + m_observer, + 12.0, + 0.020); + + // An encoder set up to measure arm position in radians. + private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel); + + private final SpeedController m_motor = new PWMVictorSPX(kMotorPort); + + // A joystick to read the trigger from. + private final Joystick m_joystick = new Joystick(kJoystickPort); + + @Override + public void robotInit() { + // We go 2 pi radians in 1 rotation, or 4096 counts. + m_encoder.setDistancePerPulse(Math.PI * 2 / 4096.0); + } + + @Override + public void teleopInit() { + // Reset our loop to make sure it's in a known state. + m_loop.reset(VecBuilder.fill(m_encoder.getDistance(), m_encoder.getRate())); + + // Reset our last reference to the current state. + m_lastProfiledReference = new TrapezoidProfile.State(m_encoder.getDistance(), + m_encoder.getRate()); + } + + @Override + public void teleopPeriodic() { + // Sets the target position of our arm. This is similar to setting the setpoint of a + // PID controller. + TrapezoidProfile.State goal; + if (m_joystick.getTrigger()) { + // the trigger is pressed, so we go to the high goal. + goal = new TrapezoidProfile.State(kRaisedPosition, 0.0); + } else { + // Otherwise, we go to the low goal + goal = new TrapezoidProfile.State(kLoweredPosition, 0.0); + } + // Step our TrapezoidalProfile forward 20ms and set it as our next reference + m_lastProfiledReference = (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference)) + .calculate(0.020); + m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity); + + // Correct our Kalman filter's state vector estimate with encoder data. + m_loop.correct(VecBuilder.fill(m_encoder.getDistance())); + + // Update our LQR to generate new voltage commands and use the voltages to predict the next + // state with out Kalman filter. + m_loop.predict(0.020); + + // Send the new calculated voltage to the motors. + // voltage = duty cycle * battery voltage, so + // duty cycle = voltage / battery voltage + double nextVoltage = m_loop.getU(0); + m_motor.setVoltage(nextVoltage); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java new file mode 100644 index 0000000000..03dff059d1 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespaceelevator; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java new file mode 100644 index 0000000000..4c59e89412 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java @@ -0,0 +1,155 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespaceelevator; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator; +import edu.wpi.first.wpilibj.estimator.KalmanFilter; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.LinearSystemLoop; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; + +/** + * This is a sample program to demonstrate how to use a state-space controller + * to control an elevator. + */ +public class Robot extends TimedRobot { + private static final int kMotorPort = 0; + private static final int kEncoderAChannel = 0; + private static final int kEncoderBChannel = 1; + private static final int kJoystickPort = 0; + private static final double kHighGoalPosition = Units.feetToMeters(3); + private static final double kLowGoalPosition = Units.feetToMeters(0); + + private static final double kCarriageMass = 4.5; // kilograms + + // A 1.5in diameter drum has a radius of 0.75in, or 0.019in. + private static final double kDrumRadius = 1.5 / 2.0 * 25.4 / 1000.0; + + // Reduction between motors and encoder, as output over input. If the elevator spins slower than + // the motors, this number should be greater than one. + private static final double kElevatorGearing = 6.0; + + private final TrapezoidProfile.Constraints m_constraints = new TrapezoidProfile.Constraints( + Units.feetToMeters(3.0), Units.feetToMeters(6.0)); // Max elevator speed and acceleration. + private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State(); + + // The plant holds a state-space model of our elevator. This system has the following properties: + // + // States: [position, velocity], in meters and meters per second. + // Inputs (what we can "put in"): [voltage], in volts. + // Outputs (what we can measure): [position], in meters. + private final LinearSystem m_elevatorPlant = LinearSystemId.createElevatorSystem( + DCMotor.getNEO(2), + kCarriageMass, + kDrumRadius, + kElevatorGearing + ); + + // The observer fuses our encoder data and voltage inputs to reject noise. + private final KalmanFilter m_observer = new KalmanFilter<>( + Nat.N2(), Nat.N1(), + m_elevatorPlant, + VecBuilder.fill(Units.inchesToMeters(2), Units.inchesToMeters(40)), // How accurate we + // think our model is, in meters and meters/second. + VecBuilder.fill(0.001), // How accurate we think our encoder position + // data is. In this case we very highly trust our encoder position reading. + 0.020); + + // A LQR uses feedback to create voltage commands. + private final LinearQuadraticRegulator m_controller + = new LinearQuadraticRegulator<>(m_elevatorPlant, + VecBuilder.fill(Units.inchesToMeters(1.0), Units.inchesToMeters(10.0)), // qelms. Position + // and velocity error tolerances, in meters and meters per second. Decrease this to more + // heavily penalize state excursion, or make the controller behave more aggressively. In + // this example we weight position much more highly than velocity, but this can be + // tuned to balance the two. + 1.0, // rho balances Q and R, or velocity and voltage weights. Increasing this + // will penalize state excursion more heavily, while decreasing this will penalize control + // effort more heavily. Useful for balancing weights for systems with more states such + // as drivetrains. + VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more + // heavily penalize control effort, or make the controller less aggressive. 12 is a good + // starting point because that is the (approximate) maximum voltage of a battery. + 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be + // lower if using notifiers. + + // The state-space loop combines a controller, observer, feedforward and plant for easy control. + private final LinearSystemLoop m_loop = new LinearSystemLoop<>( + m_elevatorPlant, + m_controller, + m_observer, + 12.0, + 0.020); + + // An encoder set up to measure elevator height in meters. + private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel); + + private final SpeedController m_motor = new PWMVictorSPX(kMotorPort); + + // A joystick to read the trigger from. + private final Joystick m_joystick = new Joystick(kJoystickPort); + + @Override + public void robotInit() { + // Circumference = pi * d, so distance per click = pi * d / counts + m_encoder.setDistancePerPulse(Math.PI * 2 * kDrumRadius / 4096.0); + } + + @Override + public void teleopInit() { + // Reset our loop to make sure it's in a known state. + m_loop.reset(VecBuilder.fill(m_encoder.getDistance(), m_encoder.getRate())); + + // Reset our last reference to the current state. + m_lastProfiledReference = new TrapezoidProfile.State(m_encoder.getDistance(), + m_encoder.getRate()); + } + + @Override + public void teleopPeriodic() { + // Sets the target position of our arm. This is similar to setting the setpoint of a + // PID controller. + TrapezoidProfile.State goal; + if (m_joystick.getTrigger()) { + // the trigger is pressed, so we go to the high goal. + goal = new TrapezoidProfile.State(kHighGoalPosition, 0.0); + } else { + // Otherwise, we go to the low goal + goal = new TrapezoidProfile.State(kLowGoalPosition, 0.0); + } + // Step our TrapezoidalProfile forward 20ms and set it as our next reference + m_lastProfiledReference = (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference)) + .calculate(0.020); + m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity); + + // Correct our Kalman filter's state vector estimate with encoder data. + m_loop.correct(VecBuilder.fill(m_encoder.getDistance())); + + // Update our LQR to generate new voltage commands and use the voltages to predict the next + // state with out Kalman filter. + m_loop.predict(0.020); + + // Send the new calculated voltage to the motors. + // voltage = duty cycle * battery voltage, so + // duty cycle = voltage / battery voltage + double nextVoltage = m_loop.getU(0); + m_motor.setVoltage(nextVoltage); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java new file mode 100644 index 0000000000..36733885cc --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespaceflywheel; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java new file mode 100644 index 0000000000..4a3a0a4d63 --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java @@ -0,0 +1,131 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespaceflywheel; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator; +import edu.wpi.first.wpilibj.estimator.KalmanFilter; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.LinearSystemLoop; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; + +/** + * This is a sample program to demonstrate how to use a state-space controller + * to control a flywheel. + */ +public class Robot extends TimedRobot { + private static final int kMotorPort = 0; + private static final int kEncoderAChannel = 0; + private static final int kEncoderBChannel = 1; + private static final int kJoystickPort = 0; + private static final double kSpinupRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(500.0); + + private static final double kFlywheelMomentOfInertia = 0.00032; // kg * m^2 + + // Reduction between motors and encoder, as output over input. If the flywheel spins slower than + // the motors, this number should be greater than one. + private static final double kFlywheelGearing = 1.0; + + // The plant holds a state-space model of our flywheel. This system has the following properties: + // + // States: [velocity], in radians per second. + // Inputs (what we can "put in"): [voltage], in volts. + // Outputs (what we can measure): [velocity], in radians per second. + private final LinearSystem m_flywheelPlant = LinearSystemId.createFlywheelSystem( + DCMotor.getNEO(2), + kFlywheelMomentOfInertia, + kFlywheelGearing); + + // The observer fuses our encoder data and voltage inputs to reject noise. + private final KalmanFilter m_observer = new KalmanFilter<>( + Nat.N1(), Nat.N1(), + m_flywheelPlant, + VecBuilder.fill(3.0), // How accurate we think our model is + VecBuilder.fill(0.01), // How accurate we think our encoder + // data is + 0.020); + + // A LQR uses feedback to create voltage commands. + private final LinearQuadraticRegulator m_controller + = new LinearQuadraticRegulator<>(m_flywheelPlant, + VecBuilder.fill(8.0), // qelms. Velocity error tolerance, in radians per second. Decrease + // this to more heavily penalize state excursion, or make the controller behave more + // aggressively. + 1.0, // rho balances Q and R, or velocity and voltage weights. Increasing this + // will penalize state excursion more heavily, while decreasing this will penalize control + // effort more heavily. Useful for balancing weights for systems with more states such + // as drivetrains. + VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more + // heavily penalize control effort, or make the controller less aggressive. 12 is a good + // starting point because that is the (approximate) maximum voltage of a battery. + 0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be + // lower if using notifiers. + + // The state-space loop combines a controller, observer, feedforward and plant for easy control. + private final LinearSystemLoop m_loop = new LinearSystemLoop<>( + m_flywheelPlant, + m_controller, + m_observer, + 12.0, + 0.020); + + // An encoder set up to measure flywheel velocity in radians per second. + private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel); + + private final SpeedController m_motor = new PWMVictorSPX(kMotorPort); + + // A joystick to read the trigger from. + private final Joystick m_joystick = new Joystick(kJoystickPort); + + @Override + public void robotInit() { + // We go 2 pi radians per 4096 clicks. + m_encoder.setDistancePerPulse( + 2.0 * Math.PI / 4096.0); + } + + @Override + public void teleopInit() { + m_loop.reset(VecBuilder.fill(m_encoder.getRate())); + } + + @Override + public void teleopPeriodic() { + // Sets the target speed of our flywheel. This is similar to setting the setpoint of a + // PID controller. + if (m_joystick.getTriggerPressed()) { + // We just pressed the trigger, so let's set our next reference + m_loop.setNextR(VecBuilder.fill(kSpinupRadPerSec)); + } else if (m_joystick.getTriggerReleased()) { + // We just released the trigger, so let's spin down + m_loop.setNextR(VecBuilder.fill(0.0)); + } + + // Correct our Kalman filter's state vector estimate with encoder data. + m_loop.correct(VecBuilder.fill(m_encoder.getRate())); + + // Update our LQR to generate new voltage commands and use the voltages to predict the next + // state with out Kalman filter. + m_loop.predict(0.020); + + // Send the new calculated voltage to the motors. + // voltage = duty cycle * battery voltage, so + // duty cycle = voltage / battery voltage + double nextVoltage = m_loop.getU(0); + m_motor.setVoltage(nextVoltage); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java new file mode 100644 index 0000000000..e487e9f09e --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespaceflywheelsysid; + +import edu.wpi.first.wpilibj.RobotBase; + +/** + * Do NOT add any static variables to this class, or any initialization at all. + * Unless you know what you are doing, do not modify this file except to + * change the parameter class to the startRobot call. + */ +public final class Main { + private Main() { + } + + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } +} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java new file mode 100644 index 0000000000..7cd773b5da --- /dev/null +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java @@ -0,0 +1,123 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.examples.statespaceflywheelsysid; + +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.PWMVictorSPX; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator; +import edu.wpi.first.wpilibj.estimator.KalmanFilter; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.LinearSystemLoop; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.util.Units; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; + +/** + * This is a sample program to demonstrate how to use a state-space controller + * to control a flywheel. + */ +public class Robot extends TimedRobot { + private static final int kMotorPort = 0; + private static final int kEncoderAChannel = 0; + private static final int kEncoderBChannel = 1; + private static final int kJoystickPort = 0; + private static final double kSpinupRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(500.0); + + // Volts per (radian per second) + private static final double kFlywheelKv = 0.023; + + // Volts per (radian per second squared) + private static final double kFlywheelKa = 0.001; + + // The plant holds a state-space model of our flywheel. This system has the following properties: + // + // States: [velocity], in radians per second. + // Inputs (what we can "put in"): [voltage], in volts. + // Outputs (what we can measure): [velocity], in radians per second. + // + // The Kv and Ka constants are found using the FRC Characterization toolsuite. + private final LinearSystem m_flywheelPlant = LinearSystemId.identifyVelocitySystem( + kFlywheelKv, kFlywheelKa); + + // The observer fuses our encoder data and voltage inputs to reject noise. + private final KalmanFilter m_observer = new KalmanFilter<>( + Nat.N1(), Nat.N1(), + m_flywheelPlant, + VecBuilder.fill(3.0), // How accurate we think our model is + VecBuilder.fill(0.01), // How accurate we think our encoder + // data is + 0.020); + + // A LQR uses feedback to create voltage commands. + private final LinearQuadraticRegulator m_controller + = new LinearQuadraticRegulator<>(m_flywheelPlant, + VecBuilder.fill(8.0), // Velocity error tolerance + VecBuilder.fill(12.0), // Control effort (voltage) tolerance + 0.020); + + // The state-space loop combines a controller, observer, feedforward and plant for easy control. + private final LinearSystemLoop m_loop = new LinearSystemLoop<>( + m_flywheelPlant, + m_controller, + m_observer, + 12.0, + 0.020); + + // An encoder set up to measure flywheel velocity in radians per second. + private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel); + + private final SpeedController m_motor = new PWMVictorSPX(kMotorPort); + + // A joystick to read the trigger from. + private final Joystick m_joystick = new Joystick(kJoystickPort); + + @Override + public void robotInit() { + // We go 2 pi radians per 4096 clicks. + m_encoder.setDistancePerPulse( + 2.0 * Math.PI / 4096.0); + } + + @Override + public void teleopInit() { + // Reset our loop to make sure it's in a known state. + m_loop.reset(VecBuilder.fill(m_encoder.getRate())); + } + + @Override + public void teleopPeriodic() { + + // Sets the target speed of our flywheel. This is similar to setting the setpoint of a + // PID controller. + if (m_joystick.getTriggerPressed()) { + // We just pressed the trigger, so let's set our next reference + m_loop.setNextR(VecBuilder.fill(kSpinupRadPerSec)); + } else if (m_joystick.getTriggerReleased()) { + // We just released the trigger, so let's spin down + m_loop.setNextR(VecBuilder.fill(0.0)); + } + + // Correct our Kalman filter's state vector estimate with encoder data. + m_loop.correct(VecBuilder.fill(m_encoder.getRate())); + + // Update our LQR to generate new voltage commands and use the voltages to predict the next + // state with out Kalman filter. + m_loop.predict(0.020); + + // Send the new calculated voltage to the motors. + // voltage = duty cycle * battery voltage, so + // duty cycle = voltage / battery voltage + double nextVoltage = m_loop.getU(0); + m_motor.setVoltage(nextVoltage); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/Drake.java b/wpimath/src/main/java/edu/wpi/first/math/Drake.java index edf360d1b2..4d66102fbb 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/Drake.java +++ b/wpimath/src/main/java/edu/wpi/first/math/Drake.java @@ -10,6 +10,7 @@ package edu.wpi.first.math; import org.ejml.simple.SimpleMatrix; import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Num; public final class Drake { private Drake() { @@ -46,13 +47,13 @@ public final class Drake { * @param R Input cost matrix. * @return Solution of DARE. */ - @SuppressWarnings("ParameterName") - public static SimpleMatrix discreteAlgebraicRiccatiEquation( - Matrix A, - Matrix B, - Matrix Q, - Matrix R) { - return discreteAlgebraicRiccatiEquation(A.getStorage(), B.getStorage(), Q.getStorage(), - R.getStorage()); + @SuppressWarnings({"ParameterName", "MethodTypeParameterName"}) + public static Matrix + discreteAlgebraicRiccatiEquation(Matrix A, + Matrix B, + Matrix Q, + Matrix R) { + return new Matrix<>(discreteAlgebraicRiccatiEquation(A.getStorage(), B.getStorage(), + Q.getStorage(), R.getStorage())); } } diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforward.java new file mode 100644 index 0000000000..79a88cdf42 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforward.java @@ -0,0 +1,215 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import java.util.function.BiFunction; +import java.util.function.Function; + +import edu.wpi.first.wpilibj.system.NumericalJacobian; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; + +/** + * Constructs a control-affine plant inversion model-based feedforward from + * given model dynamics. + * + *

If given the vector valued function as f(x, u) where x is the state + * vector and u is the input vector, the B matrix(continuous input matrix) + * is calculated through a {@link edu.wpi.first.wpilibj.system.NumericalJacobian}. + * In this case f has to be control-affine (of the form f(x) + Bu). + * + *

The feedforward is calculated as + * u_ff = B+ (rDot - f(x)), where + * B+ is the pseudoinverse of B. + * + *

This feedforward does not account for a dynamic B matrix, B is either + * determined or supplied when the feedforward is created and remains constant. + * + *

For more on the underlying math, read + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf. + */ +@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"}) +public class ControlAffinePlantInversionFeedforward { + /** + * The current reference state. + */ + @SuppressWarnings("MemberName") + private Matrix m_r; + + /** + * The computed feedforward. + */ + private Matrix m_uff; + + @SuppressWarnings("MemberName") + private final Matrix m_B; + + private final Nat m_inputs; + + private final double m_dt; + + /** + * The model dynamics. + */ + private final BiFunction, Matrix, Matrix> m_f; + + /** + * Constructs a feedforward with given model dynamics as a function + * of state and input. + * + * @param states A {@link Nat} representing the number of states. + * @param inputs A {@link Nat} representing the number of inputs. + * @param f A vector-valued function of x, the state, and + * u, the input, that returns the derivative of + * the state vector. HAS to be control-affine + * (of the form f(x) + Bu). + * @param dtSeconds The timestep between calls of calculate(). + */ + public ControlAffinePlantInversionFeedforward( + Nat states, + Nat inputs, + BiFunction, Matrix, Matrix> f, + double dtSeconds) { + this.m_dt = dtSeconds; + this.m_f = f; + this.m_inputs = inputs; + + this.m_B = NumericalJacobian.numericalJacobianU(states, inputs, + m_f, new Matrix<>(states, Nat.N1()), new Matrix<>(inputs, Nat.N1())); + + m_r = new Matrix<>(states, Nat.N1()); + m_uff = new Matrix<>(inputs, Nat.N1()); + + reset(m_r); + } + + /** + * Constructs a feedforward with given model dynamics as a function of state, + * and the plant's B(continuous input matrix) matrix. + * + * @param states A {@link Nat} representing the number of states. + * @param inputs A {@link Nat} representing the number of inputs. + * @param f A vector-valued function of x, the state, + * that returns the derivative of the state vector. + * @param B Continuous input matrix of the plant being controlled. + * @param dtSeconds The timestep between calls of calculate(). + */ + public ControlAffinePlantInversionFeedforward( + Nat states, + Nat inputs, + Function, Matrix> f, + Matrix B, + double dtSeconds) { + this.m_dt = dtSeconds; + this.m_inputs = inputs; + + this.m_f = (x, u) -> f.apply(x); + this.m_B = B; + + m_r = new Matrix<>(states, Nat.N1()); + m_uff = new Matrix<>(inputs, Nat.N1()); + + reset(m_r); + } + + + /** + * Returns the previously calculated feedforward as an input vector. + * + * @return The calculated feedforward. + */ + public Matrix getUff() { + return m_uff; + } + + /** + * Returns an element of the previously calculated feedforward. + * + * @param row Row of uff. + * + * @return The row of the calculated feedforward. + */ + public double getUff(int row) { + return m_uff.get(row, 0); + } + + /** + * Returns the current reference vector r. + * + * @return The current reference vector. + */ + public Matrix getR() { + return m_r; + } + + /** + * Returns an element of the current reference vector r. + * + * @param row Row of r. + * + * @return The row of the current reference vector. + */ + public double getR(int row) { + return m_r.get(row, 0); + } + + /** + * Resets the feedforward with a specified initial state vector. + * + * @param initialState The initial state vector. + */ + public void reset(Matrix initialState) { + m_r = initialState; + m_uff.fill(0.0); + } + + /** + * Resets the feedforward with a zero initial state vector. + */ + public void reset() { + m_r.fill(0.0); + m_uff.fill(0.0); + } + + /** + * Calculate the feedforward with only the desired + * future reference. This uses the internally stored "current" + * reference. + * + *

If this method is used the initial state of the system is the one + * set using {@link LinearPlantInversionFeedforward#reset(Matrix)}. + * If the initial state is not set it defaults to a zero vector. + * + * @param nextR The reference state of the future timestep (k + dt). + * + * @return The calculated feedforward. + */ + public Matrix calculate(Matrix nextR) { + return calculate(m_r, nextR); + } + + /** + * Calculate the feedforward with current and future reference vectors. + * + * @param r The reference state of the current timestep (k). + * @param nextR The reference state of the future timestep (k + dt). + * + * @return The calculated feedforward. + */ + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + public Matrix calculate(Matrix r, Matrix nextR) { + var rDot = (nextR.minus(r)).div(m_dt); + + m_uff = m_B.solve(rDot.minus(m_f.apply(r, new Matrix<>(m_inputs, Nat.N1())))); + + m_r = nextR; + return m_uff; + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforward.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforward.java new file mode 100644 index 0000000000..8d65165100 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforward.java @@ -0,0 +1,170 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.wpilibj.math.Discretization; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; + +/** + * Constructs a plant inversion model-based feedforward from a {@link LinearSystem}. + * + *

The feedforward is calculated as u_ff = B+ (r_k+1 - A r_k) , + * where B+ is the pseudoinverse of B. + * + *

For more on the underlying math, read + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf. + */ +@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"}) +public class LinearPlantInversionFeedforward { + /** + * The current reference state. + */ + @SuppressWarnings("MemberName") + private Matrix m_r; + + /** + * The computed feedforward. + */ + private Matrix m_uff; + + @SuppressWarnings("MemberName") + private Matrix m_B; + + @SuppressWarnings("MemberName") + private Matrix m_A; + + /** + * Constructs a feedforward with the given plant. + * + * @param plant The plant being controlled. + * @param dtSeconds Discretization timestep. + */ + public LinearPlantInversionFeedforward( + LinearSystem plant, + double dtSeconds + ) { + this(plant.getA(), plant.getB(), dtSeconds); + } + + /** + * Constructs a feedforward with the given coefficients. + * + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param dtSeconds Discretization timestep. + */ + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + public LinearPlantInversionFeedforward(Matrix A, Matrix B, + double dtSeconds) { + var discABPair = Discretization.discretizeAB(A, B, dtSeconds); + this.m_A = discABPair.getFirst(); + this.m_B = discABPair.getSecond(); + + m_r = new Matrix(new SimpleMatrix(B.getNumRows(), 1)); + m_uff = new Matrix(new SimpleMatrix(B.getNumCols(), 1)); + + reset(m_r); + } + + /** + * Returns the previously calculated feedforward as an input vector. + * + * @return The calculated feedforward. + */ + public Matrix getUff() { + return m_uff; + } + + /** + * Returns an element of the previously calculated feedforward. + * + * @param row Row of uff. + * + * @return The row of the calculated feedforward. + */ + public double getUff(int row) { + return m_uff.get(row, 0); + } + + /** + * Returns the current reference vector r. + * + * @return The current reference vector. + */ + public Matrix getR() { + return m_r; + } + + /** + * Returns an element of the current reference vector r. + * + * @param row Row of r. + * + * @return The row of the current reference vector. + */ + public double getR(int row) { + return m_r.get(row, 0); + } + + /** + * Resets the feedforward with a specified initial state vector. + * + * @param initialState The initial state vector. + */ + public void reset(Matrix initialState) { + m_r = initialState; + m_uff.fill(0.0); + } + + /** + * Resets the feedforward with a zero initial state vector. + */ + public void reset() { + m_r.fill(0.0); + m_uff.fill(0.0); + } + + /** + * Calculate the feedforward with only the desired + * future reference. This uses the internally stored "current" + * reference. + * + *

If this method is used the initial state of the system is the one + * set using {@link LinearPlantInversionFeedforward#reset(Matrix)}. + * If the initial state is not set it defaults to a zero vector. + * + * @param nextR The reference state of the future timestep (k + dt). + * + * @return The calculated feedforward. + */ + public Matrix calculate(Matrix nextR) { + return calculate(m_r, nextR); + } + + /** + * Calculate the feedforward with current and future reference vectors. + * + * @param r The reference state of the current timestep (k). + * @param nextR The reference state of the future timestep (k + dt). + * + * @return The calculated feedforward. + */ + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + public Matrix calculate(Matrix r, Matrix nextR) { + m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r)))); + + m_r = nextR; + return m_uff; + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java new file mode 100644 index 0000000000..1db5eba5e1 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java @@ -0,0 +1,254 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.math.Drake; +import edu.wpi.first.wpilibj.math.Discretization; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.Vector; +import edu.wpi.first.wpiutil.math.numbers.N1; + + +/** + * Contains the controller coefficients and logic for a linear-quadratic + * regulator (LQR). + * LQRs use the control law u = K(r - x). + * + *

For more on the underlying math, read + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf. + */ +@SuppressWarnings("ClassTypeParameterName") +public class LinearQuadraticRegulator { + /** + * The current reference state. + */ + @SuppressWarnings("MemberName") + private Matrix m_r; + + /** + * The computed and capped controller output. + */ + @SuppressWarnings("MemberName") + private Matrix m_u; + + // Controller gain. + @SuppressWarnings("MemberName") + private Matrix m_K; + + /** + * Constructs a controller with the given coefficients and plant. Rho is defaulted to 1. + * + * @param plant The plant being controlled. + * @param qelms The maximum desired error tolerance for each state. + * @param relms The maximum desired control effort for each input. + * @param dtSeconds Discretization timestep. + */ + public LinearQuadraticRegulator( + LinearSystem plant, + Vector qelms, + Vector relms, + double dtSeconds + ) { + this(plant.getA(), plant.getB(), qelms, 1.0, relms, dtSeconds); + } + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param plant The plant being controlled. + * @param qelms The maximum desired error tolerance for each state. + * @param rho A weighting factor that balances control effort and state excursion. + * Greater values penalize state excursion more heavily. 1 is a good starting + * value. + * @param relms The maximum desired control effort for each input. + * @param dtSeconds Discretization timestep. + */ + public LinearQuadraticRegulator( + LinearSystem plant, + Vector qelms, + double rho, + Vector relms, + double dtSeconds + ) { + this(plant.getA(), plant.getB(), qelms, rho, relms, dtSeconds); + } + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param qelms The maximum desired error tolerance for each state. + * @param relms The maximum desired control effort for each input. + * @param dtSeconds Discretization timestep. + */ + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + public LinearQuadraticRegulator(Matrix A, Matrix B, + Vector qelms, Vector relms, + double dtSeconds + ) { + this(A, B, qelms, 1.0, relms, dtSeconds); + } + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param qelms The maximum desired error tolerance for each state. + * @param rho A weighting factor that balances control effort and state excursion. + * Greater + * values penalize state excursion more heavily. 1 is a good starting value. + * @param relms The maximum desired control effort for each input. + * @param dtSeconds Discretization timestep. + */ + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + public LinearQuadraticRegulator(Matrix A, Matrix B, + Vector qelms, double rho, Vector relms, + double dtSeconds + ) { + this(A, B, StateSpaceUtil.makeCostMatrix(qelms).times(rho), + StateSpaceUtil.makeCostMatrix(relms), dtSeconds); + } + + /** + * Constructs a controller with the given coefficients and plant. + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param Q The state cost matrix. + * @param R The input cost matrix. + * @param dtSeconds Discretization timestep. + */ + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + public LinearQuadraticRegulator(Matrix A, Matrix B, + Matrix Q, Matrix R, + double dtSeconds + ) { + var discABPair = Discretization.discretizeAB(A, B, dtSeconds); + var discA = discABPair.getFirst(); + var discB = discABPair.getSecond(); + + var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R); + + var temp = discB.transpose().times(S).times(discB).plus(R); + + m_K = temp.solve(discB.transpose().times(S).times(discA)); + + m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1)); + m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1)); + + reset(); + } + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param states The number of states. + * @param inputs The number of inputs. + * @param k The gain matrix. + */ + @SuppressWarnings("ParameterName") + public LinearQuadraticRegulator( + Nat states, Nat inputs, + Matrix k + ) { + m_K = k; + + m_r = new Matrix<>(states, Nat.N1()); + m_u = new Matrix<>(inputs, Nat.N1()); + + reset(); + } + + /** + * Returns the control input vector u. + * + * @return The control input. + */ + public Matrix getU() { + return m_u; + } + + /** + * Returns an element of the control input vector u. + * + * @param row Row of u. + * + * @return The row of the control input vector. + */ + public double getU(int row) { + return m_u.get(row, 0); + } + + /** + * Returns the reference vector r. + * + * @return The reference vector. + */ + public Matrix getR() { + return m_r; + } + + /** + * Returns an element of the reference vector r. + * + * @param row Row of r. + * + * @return The row of the reference vector. + */ + public double getR(int row) { + return m_r.get(row, 0); + } + + /** + * Returns the controller matrix K. + * + * @return the controller matrix K. + */ + public Matrix getK() { + return m_K; + } + + /** + * Resets the controller. + */ + public void reset() { + m_r.fill(0.0); + m_u.fill(0.0); + } + + /** + * Returns the next output of the controller. + * + * @param x The current state x. + */ + @SuppressWarnings("ParameterName") + public Matrix calculate(Matrix x) { + m_u = m_K.times(m_r.minus(x)); + return m_u; + } + + /** + * Returns the next output of the controller. + * + * @param x The current state x. + * @param nextR the next reference vector r. + */ + @SuppressWarnings("ParameterName") + public Matrix calculate(Matrix x, Matrix nextR) { + m_r = nextR; + return calculate(x); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java new file mode 100644 index 0000000000..68a84e89aa --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java @@ -0,0 +1,286 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.estimator; + +import java.util.function.BiFunction; + +import edu.wpi.first.math.Drake; +import edu.wpi.first.wpilibj.math.Discretization; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpilibj.system.NumericalJacobian; +import edu.wpi.first.wpilibj.system.RungeKutta; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; + +/** + * Kalman filters combine predictions from a model and measurements to give an estimate of the true + * system state. This is useful because many states cannot be measured directly as a result of + * sensor noise, or because the state is "hidden". + * + *

The Extended Kalman filter is just like the {@link KalmanFilter Kalman filter}, but we make a + * linear approximation of nonlinear dynamics and/or nonlinear measurement models. This means that + * the EKF works with nonlinear systems. + */ +@SuppressWarnings("ClassTypeParameterName") +public class ExtendedKalmanFilter + implements KalmanTypeFilter { + private final Nat m_states; + private final Nat m_outputs; + + @SuppressWarnings("MemberName") + private final BiFunction, Matrix, Matrix> m_f; + @SuppressWarnings("MemberName") + private final BiFunction, Matrix, Matrix> m_h; + private final Matrix m_contQ; + private Matrix m_discR; + private final Matrix m_initP; + private final Matrix m_contR; + @SuppressWarnings("MemberName") + private Matrix m_xHat; + @SuppressWarnings("MemberName") + private Matrix m_P; + + /** + * Constructs an extended Kalman filter. + * + * @param states a Nat representing the number of states. + * @param inputs a Nat representing the number of inputs. + * @param outputs a Nat representing the number of outputs. + * @param f A vector-valued function of x and u that returns + * the derivative of the state vector. + * @param h A vector-valued function of x and u that returns + * the measurement vector. + * @param stateStdDevs Standard deviations of model states. + * @param measurementStdDevs Standard deviations of measurements. + * @param dtSeconds Nominal discretization timestep. + */ + @SuppressWarnings("ParameterName") + public ExtendedKalmanFilter( + Nat states, + Nat inputs, + Nat outputs, + BiFunction, Matrix, Matrix> f, + BiFunction, Matrix, Matrix> h, + Matrix stateStdDevs, + Matrix measurementStdDevs, + double dtSeconds + ) { + m_states = states; + m_outputs = outputs; + + m_f = f; + m_h = h; + + reset(); + + m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs); + this.m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs); + + final var contA = NumericalJacobian + .numericalJacobianX(states, states, f, m_xHat, new Matrix<>(inputs, Nat.N1())); + final var C = NumericalJacobian + .numericalJacobianX(outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1())); + + final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds); + final var discA = discPair.getFirst(); + final var discQ = discPair.getSecond(); + + m_discR = Discretization.discretizeR(m_contR, dtSeconds); + + // IsStabilizable(A^T, C^T) will tell us if the system is observable. + boolean isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose()); + if (isObservable && outputs.getNum() <= states.getNum()) { + m_initP = Drake.discreteAlgebraicRiccatiEquation( + discA.transpose(), C.transpose(), discQ, m_discR) ; + } else { + m_initP = new Matrix<>(states, states); + } + + m_P = m_initP; + } + + /** + * Returns the error covariance matrix P. + * + * @return the error covariance matrix P. + */ + @Override + public Matrix getP() { + return m_P; + } + + /** + * Returns an element of the error covariance matrix P. + * + * @param row Row of P. + * @param col Column of P. + * @return the value of the error covariance matrix P at (i, j). + */ + @Override + public double getP(int row, int col) { + return m_P.get(row, col); + } + + /** + * Sets the entire error covariance matrix P. + * + * @param newP The new value of P to use. + */ + @Override + public void setP(Matrix newP) { + m_P = newP; + } + + /** + * Returns the state estimate x-hat. + * + * @return the state estimate x-hat. + */ + @Override + public Matrix getXhat() { + return m_xHat; + } + + /** + * Returns an element of the state estimate x-hat. + * + * @param row Row of x-hat. + * @return the value of the state estimate x-hat at i. + */ + @Override + public double getXhat(int row) { + return m_xHat.get(row, 0); + } + + /** + * Set initial state estimate x-hat. + * + * @param xHat The state estimate x-hat. + */ + @SuppressWarnings("ParameterName") + @Override + public void setXhat(Matrix xHat) { + m_xHat = xHat; + } + + + /** + * Set an element of the initial state estimate x-hat. + * + * @param row Row of x-hat. + * @param value Value for element of x-hat. + */ + @Override + public void setXhat(int row, double value) { + m_xHat.set(row, 0, value); + } + + @Override + public void reset() { + m_xHat = new Matrix<>(m_states, Nat.N1()); + m_P = m_initP; + } + + /** + * Project the model into the future with a new control input u. + * + * @param u New control input from controller. + * @param dtSeconds Timestep for prediction. + */ + @SuppressWarnings("ParameterName") + @Override + public void predict(Matrix u, double dtSeconds) { + predict(u, m_f, dtSeconds); + } + + /** + * Project the model into the future with a new control input u. + * + * @param u New control input from controller. + * @param f The function used to linearlize the model. + * @param dtSeconds Timestep for prediction. + */ + @SuppressWarnings("ParameterName") + public void predict( + Matrix u, BiFunction, + Matrix, Matrix> f, + double dtSeconds + ) { + // Find continuous A + final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u); + + // Find discrete A and Q + final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds); + final var discA = discPair.getFirst(); + final var discQ = discPair.getSecond(); + + m_xHat = RungeKutta.rungeKutta(f, m_xHat, u, dtSeconds); + m_P = discA.times(m_P).times(discA.transpose()).plus(discQ); + m_discR = Discretization.discretizeR(m_contR, dtSeconds); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * @param u Same control input used in the predict step. + * @param y Measurement vector. + */ + @SuppressWarnings("ParameterName") + @Override + public void correct(Matrix u, Matrix y) { + correct(m_outputs, u, y, m_h, m_discR); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + *

This is useful for when the measurements available during a timestep's + * Correct() call vary. The h(x, u) passed to the constructor is used if one is + * not provided (the two-argument version of this function). + * + * @param Number of rows in the result of f(x, u). + * @param rows Number of rows in the result of f(x, u). + * @param u Same control input used in the predict step. + * @param y Measurement vector. + * @param h A vector-valued function of x and u that returns the measurement + * vector. + * @param R Discrete measurement noise covariance matrix. + */ + @SuppressWarnings({"ParameterName", "MethodTypeParameterName"}) + public void correct( + Nat rows, Matrix u, + Matrix y, + BiFunction, Matrix, Matrix> h, + Matrix R + ) { + final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u); + + final var S = C.times(m_P).times(C.transpose()).plus(R); + + // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more + // efficiently. + // + // K = PC^T S^-1 + // KS = PC^T + // (KS)^T = (PC^T)^T + // S^T K^T = CP^T + // + // The solution of Ax = b can be found via x = A.solve(b). + // + // K^T = S^T.solve(CP^T) + // K = (S^T.solve(CP^T))^T + // + // Now we have the Kalman gain + final Matrix K = S.transpose().solve(C.times(m_P.transpose())).transpose(); + + m_xHat = m_xHat.plus(K.times(y.minus(h.apply(m_xHat, u)))); + m_P = Matrix.eye(m_states).minus(K.times(C)).times(m_P); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilter.java new file mode 100644 index 0000000000..a03dae5c37 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilter.java @@ -0,0 +1,290 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.estimator; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.math.Drake; +import edu.wpi.first.math.MathSharedStore; +import edu.wpi.first.wpilibj.math.Discretization; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; + +/** + * A Kalman filter combines predictions from a model and measurements to give an estimate of the + * true system state. This is useful because many states cannot be measured directly as a result of + * sensor noise, or because the state is "hidden". + * + *

Kalman filters use a K gain matrix to determine whether to trust the model or measurements + * more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum + * of squares error in the state estimate. This K gain is used to correct the state estimate by + * some amount of the difference between the actual measurements and the measurements predicted by + * the model. + * + *

For more on the underlying math, read + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9. + */ +@SuppressWarnings("ClassTypeParameterName") +public class KalmanFilter implements KalmanTypeFilter { + + private final Nat m_states; + + private final LinearSystem m_plant; + + private final Matrix m_stateStdDevs; + private final Matrix m_measurementStdDevs; + + /** + * Error covariance matrix. + */ + @SuppressWarnings("MemberName") + private Matrix m_P; + + /** + * Continuous process noise covariance matrix. + */ + private final Matrix m_contQ; + + /** + * Continuous measurement noise covariance matrix. + */ + private final Matrix m_contR; + + /** + * Discrete measurement noise covariance matrix. + */ + private Matrix m_discR; + + /** + * The current state estimate x-hat. + */ + @SuppressWarnings("MemberName") + private Matrix m_xHat; + + /** + * Constructs a state-space observer with the given plant. + * + * @param states A Nat representing the states of the system. + * @param outputs A Nat representing the outputs of the system. + * @param plant The plant used for the prediction step. + * @param stateStdDevs Standard deviations of model states. + * @param measurementStdDevs Standard deviations of measurements. + * @param dtSeconds Nominal discretization timestep. + */ + public KalmanFilter( + Nat states, Nat outputs, + LinearSystem plant, + Matrix stateStdDevs, + Matrix measurementStdDevs, + double dtSeconds + ) { + this.m_states = states; + + this.m_plant = plant; + + this.m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs); + this.m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs); + + this.m_stateStdDevs = stateStdDevs; + this.m_measurementStdDevs = measurementStdDevs; + + var pair = Discretization.discretizeAQTaylor(plant.getA(), m_contQ, dtSeconds); + var discA = pair.getFirst(); + var discQ = pair.getSecond(); + + m_discR = Discretization.discretizeR(m_contR, dtSeconds); + + // IsStabilizable(A^T, C^T) will tell us if the system is observable. + var isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), plant.getC().transpose()); + if (isObservable) { + if (outputs.getNum() <= states.getNum()) { + m_P = Drake.discreteAlgebraicRiccatiEquation( + discA.transpose(), plant.getC().transpose(), discQ, m_discR); + } else { + m_P = new Matrix<>(new SimpleMatrix(states.getNum(), states.getNum())); + } + } else { + MathSharedStore.reportError("The system passed to the Kalman Filter is not observable!", + Thread.currentThread().getStackTrace()); + throw new IllegalArgumentException( + "The system passed to the Kalman Filter is not observable!"); + } + + reset(); + } + + @Override + public void reset() { + m_xHat = new Matrix<>(m_states, Nat.N1()); + } + + /** + * Returns the error covariance matrix P. + * + * @return the error covariance matrix P. + */ + @Override + public Matrix getP() { + return m_P; + } + + /** + * Returns an element of the error covariance matrix P. + * + * @param row Row of P. + * @param col Column of P. + * @return the element (i, j) of the error covariance matrix P. + */ + @Override + public double getP(int row, int col) { + return m_P.get(row, col); + } + + /** + * Sets the entire error covariance matrix P. + * + * @param newP The new value of P to use. + */ + @Override + public void setP(Matrix newP) { + m_P = newP; + } + + /** + * Set initial state estimate x-hat. + * + * @param xhat The state estimate x-hat. + */ + @Override + public void setXhat(Matrix xhat) { + this.m_xHat = xhat; + } + + /** + * Set an element of the initial state estimate x-hat. + * + * @param row Row of x-hat. + * @param value Value for element of x-hat. + */ + @Override + public void setXhat(int row, double value) { + m_xHat.set(row, 0, value); + } + + /** + * Returns the state estimate x-hat. + * + * @return The state estimate x-hat. + */ + @Override + public Matrix getXhat() { + return m_xHat; + } + + /** + * Returns an element of the state estimate x-hat. + * + * @param row Row of x-hat. + * @return the state estimate x-hat at i. + */ + @Override + public double getXhat(int row) { + return m_xHat.get(row, 0); + } + + /** + * Returns the state standard deviations used to make Q. + */ + public Matrix getStateStdDevs() { + return m_stateStdDevs; + } + + /** + * Returns the measurement standard deviations used to make R. + */ + public Matrix getMeasurementStdDevs() { + return m_measurementStdDevs; + } + + /** + * Project the model into the future with a new control input u. + * + * @param u New control input from controller. + * @param dtSeconds Timestep for prediction. + */ + @SuppressWarnings("ParameterName") + @Override + public void predict(Matrix u, double dtSeconds) { + this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds); + + var pair = Discretization.discretizeAQTaylor(m_plant.getA(), m_contQ, dtSeconds); + var discA = pair.getFirst(); + var discQ = pair.getSecond(); + + m_P = discA.times(m_P).times(discA.transpose()).plus(discQ); + m_discR = Discretization.discretizeR(m_contR, dtSeconds); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * @param u Same control input used in the last predict step. + * @param y Measurement vector. + */ + @SuppressWarnings("ParameterName") + @Override + public void correct(Matrix u, Matrix y) { + correct(u, y, m_plant.getC(), m_plant.getD(), m_discR); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + *

This is useful for when the measurements available during a timestep's + * Correct() call vary. The C matrix passed to the constructor is used if one + * is not provided (the two-argument version of this function). + * + * @param Number of rows in the result of f(x, u). + * @param u Same control input used in the predict step. + * @param y Measurement vector. + * @param C Output matrix. + * @param r Discrete measurement noise covariance matrix. + */ + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + public void correct( + Matrix u, + Matrix y, + Matrix C, + Matrix D, + Matrix r) { + var S = C.times(m_P).times(C.transpose()).plus(r); + + // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more + // efficiently. + // + // K = PC^T S^-1 + // KS = PC^T + // (KS)^T = (PC^T)^T + // S^T K^T = CP^T + // + // The solution of Ax = b can be found via x = A.solve(b). + // + // K^T = S^T.solve(CP^T) + // K = (S^T.solve(CP^T))^T + Matrix K = S.transpose().solve(C.times(m_P.transpose())).transpose(); + + m_xHat = m_xHat.plus(K.times(y.minus(C.times(m_xHat).plus(D.times(u))))); + m_P = Matrix.eye(m_states).minus(K.times(C)).times(m_P); + } + +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java new file mode 100644 index 0000000000..aa93b29588 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.estimator; + +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; + +@SuppressWarnings({"ParameterName", "InterfaceTypeParameterName"}) +interface KalmanTypeFilter { + Matrix getP(); + + double getP(int i, int j); + + void setP(Matrix newP); + + Matrix getXhat(); + + double getXhat(int i); + + void setXhat(Matrix xHat); + + void setXhat(int i, double value); + + void reset(); + + void predict(Matrix u, double dtSeconds); + + void correct(Matrix u, Matrix y); +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java new file mode 100644 index 0000000000..56e9288c07 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java @@ -0,0 +1,168 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.estimator; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; + +/** + * Generates sigma points and weights according to Van der Merwe's 2004 + * dissertation[1] for the UnscentedKalmanFilter class. + * + *

It parametrizes the sigma points using alpha, beta, kappa terms, and is the + * version seen in most publications. Unless you know better, this should be + * your default choice. + * + *

States is the dimensionality of the state. 2*States+1 weights will be + * generated. + * + *

[1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic + * Inference in Dynamic State-Space Models" (Doctoral dissertation) + */ +public class MerweScaledSigmaPoints { + + private final double m_alpha; + private final int m_kappa; + private final Nat m_states; + private Matrix m_wm; + private Matrix m_wc; + + /** + * Constructs a generator for Van der Merwe scaled sigma points. + * + * @param states an instance of Num that represents the number of states. + * @param alpha Determines the spread of the sigma points around the mean. + * Usually a small positive value (1e-3). + * @param beta Incorporates prior knowledge of the distribution of the mean. + * For Gaussian distributions, beta = 2 is optimal. + * @param kappa Secondary scaling parameter usually set to 0 or 3 - States. + */ + public MerweScaledSigmaPoints(Nat states, double alpha, double beta, int kappa) { + this.m_states = states; + this.m_alpha = alpha; + this.m_kappa = kappa; + + computeWeights(beta); + } + + /** + * Constructs a generator for Van der Merwe scaled sigma points with default values for alpha, + * beta, and kappa. + * + * @param states an instance of Num that represents the number of states. + */ + public MerweScaledSigmaPoints(Nat states) { + this(states, 1e-3, 2, 3 - states.getNum()); + } + + /** + * Returns number of sigma points for each variable in the state x. + * + * @return The number of sigma points for each variable in the state x. + */ + public int getNumSigmas() { + return 2 * m_states.getNum() + 1; + } + + /** + * Computes the sigma points for an unscented Kalman filter given the mean + * (x) and covariance(P) of the filter. + * + * @param x An array of the means. + * @param P Covariance of the filter. + * @return Two dimensional array of sigma points. Each column contains all of + * the sigmas for one dimension in the problem space. Ordered by + * Xi_0, Xi_{1..n}, Xi_{n+1..2n}. + */ + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + public Matrix sigmaPoints( + Matrix x, + Matrix P) { + double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum(); + + var intermediate = P.times(lambda + m_states.getNum()); + var U = intermediate.lltDecompose(true); // Lower triangular + + // 2 * states + 1 by states + Matrix sigmas = new Matrix<>( + new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1)); + sigmas.setColumn(0, x); + for (int k = 0; k < m_states.getNum(); k++) { + var xPlusU = x.plus(U.extractColumnVector(k)); + var xMinusU = x.minus(U.extractColumnVector(k)); + sigmas.setColumn(k + 1, xPlusU); + sigmas.setColumn(m_states.getNum() + k + 1, xMinusU); + } + + return new Matrix<>(sigmas); + } + + /** + * Computes the weights for the scaled unscented Kalman filter. + * + * @param beta Incorporates prior knowledge of the distribution of the mean. + */ + @SuppressWarnings("LocalVariableName") + private void computeWeights(double beta) { + double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum(); + double c = 0.5 / (m_states.getNum() + lambda); + + Matrix wM = new Matrix<>(new SimpleMatrix(2 * m_states.getNum() + 1, 1)); + Matrix wC = new Matrix<>(new SimpleMatrix(2 * m_states.getNum() + 1, 1)); + wM.fill(c); + wC.fill(c); + + wM.set(0, 0, lambda / (m_states.getNum() + lambda)); + wC.set(0, 0, lambda / (m_states.getNum() + lambda) + (1 - Math.pow(m_alpha, 2) + beta)); + + this.m_wm = wM; + this.m_wc = wC; + } + + /** + * Returns the weight for each sigma point for the mean. + * + * @return the weight for each sigma point for the mean. + */ + public Matrix getWm() { + return m_wm; + } + + /** + * Returns an element of the weight for each sigma point for the mean. + * + * @param element Element of vector to return. + * @return the element i's weight for the mean. + */ + public double getWm(int element) { + return m_wm.get(element, 0); + } + + /** + * Returns the weight for each sigma point for the covariance. + * + * @return the weight for each sigma point for the covariance. + */ + public Matrix getWc() { + return m_wc; + } + + /** + * Returns an element of the weight for each sigma point for the covariance. + * + * @param element Element of vector to return. + * @return The element I's weight for the covariance. + */ + public double getWc(int element) { + return m_wc.get(element, 0); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java new file mode 100644 index 0000000000..bac4511427 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java @@ -0,0 +1,314 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.estimator; + +import java.util.function.BiFunction; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.wpilibj.math.Discretization; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpilibj.system.NumericalJacobian; +import edu.wpi.first.wpilibj.system.RungeKutta; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.Pair; +import edu.wpi.first.wpiutil.math.numbers.N1; + +/** + * A Kalman filter combines predictions from a model and measurements to give an estimate of the + * true ystem state. This is useful because many states cannot be measured directly as a result of + * sensor noise, or because the state is "hidden". + * + *

The Unscented Kalman filter is similar to the {@link KalmanFilter Kalman filter}, except that + * it propagates carefully chosen points called sigma points through the non-linear model to obtain + * an estimate of the true covariance (as opposed to a linearized version of it). This means that + * the UKF works with nonlinear systems. + */ +@SuppressWarnings({"MemberName", "ClassTypeParameterName"}) +public class UnscentedKalmanFilter implements KalmanTypeFilter { + + private final Nat m_states; + private final Nat m_outputs; + + private final BiFunction, Matrix, Matrix> m_f; + private final BiFunction, Matrix, Matrix> m_h; + + private Matrix m_xHat; + private Matrix m_P; + private final Matrix m_contQ; + private final Matrix m_contR; + private Matrix m_discR; + private Matrix m_sigmasF; + + private final MerweScaledSigmaPoints m_pts; + + /** + * Constructs an Unscented Kalman Filter. + * + * @param states A Nat representing the number of states. + * @param outputs A Nat representing the number of outputs. + * @param f A vector-valued function of x and u that returns + * the derivative of the state vector. + * @param h A vector-valued function of x and u that returns + * the measurement vector. + * @param stateStdDevs Standard deviations of model states. + * @param measurementStdDevs Standard deviations of measurements. + * @param nominalDtSeconds Nominal discretization timestep. + */ + @SuppressWarnings("ParameterName") + public UnscentedKalmanFilter(Nat states, Nat outputs, + BiFunction, Matrix, + Matrix> f, + BiFunction, Matrix, + Matrix> h, + Matrix stateStdDevs, + Matrix measurementStdDevs, + double nominalDtSeconds) { + this.m_states = states; + this.m_outputs = outputs; + + m_f = f; + m_h = h; + + m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs); + m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs); + + m_discR = Discretization.discretizeR(m_contR, nominalDtSeconds); + + m_pts = new MerweScaledSigmaPoints<>(states); + + reset(); + } + + @SuppressWarnings({"ParameterName", "LocalVariableName", "PMD.CyclomaticComplexity"}) + static + Pair, Matrix> unscentedTransform( + Nat s, Nat dim, Matrix sigmas, Matrix Wm, Matrix Wc + ) { + if (sigmas.getNumRows() != dim.getNum() || sigmas.getNumCols() != 2 * s.getNum() + 1) { + throw new IllegalArgumentException("Sigmas must be covDim by 2 * states + 1! Got " + + sigmas.getNumRows() + " by " + sigmas.getNumCols()); + } + + if (Wm.getNumRows() != 2 * s.getNum() + 1 || Wm.getNumCols() != 1 ) { + throw new IllegalArgumentException("Wm must be 2 * states + 1 by 1! Got " + + Wm.getNumRows() + " by " + Wm.getNumCols()); + } + + if (Wc.getNumRows() != 2 * s.getNum() + 1 || Wc.getNumCols() != 1) { + throw new IllegalArgumentException("Wc must be 2 * states + 1 by 1! Got " + + Wc.getNumRows() + " by " + Wc.getNumCols()); + } + + // New mean is just the sum of the sigmas * weight + // dot = \Sigma^n_1 (W[k]*Xi[k]) + Matrix x = sigmas.times(Matrix.changeBoundsUnchecked(Wm)); + + // New covariance is the sum of the outer product of the residuals times the + // weights + Matrix y = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + 1)); + for (int i = 0; i < 2 * s.getNum() + 1; i++) { + y.setColumn(i, sigmas.extractColumnVector(i).minus(x)); + } + Matrix P = y.times(Matrix.changeBoundsUnchecked(Wc.diag())) + .times(Matrix.changeBoundsUnchecked(y.transpose())); + + return new Pair<>(x, P); + } + + /** + * Returns the error covariance matrix P. + * + * @return the error covariance matrix P. + */ + @Override + public Matrix getP() { + return m_P; + } + + /** + * Returns an element of the error covariance matrix P. + * + * @param row Row of P. + * @param col Column of P. + * @return the value of the error covariance matrix P at (i, j). + */ + @Override + public double getP(int row, int col) { + return m_P.get(row, col); + } + + /** + * Sets the entire error covariance matrix P. + * + * @param newP The new value of P to use. + */ + @Override + public void setP(Matrix newP) { + m_P = newP; + } + + /** + * Returns the state estimate x-hat. + * + * @return the state estimate x-hat. + */ + @Override + public Matrix getXhat() { + return m_xHat; + } + + /** + * Returns an element of the state estimate x-hat. + * + * @param row Row of x-hat. + * @return the value of the state estimate x-hat at i. + */ + @Override + public double getXhat(int row) { + return m_xHat.get(row, 0); + } + + + /** + * Set initial state estimate x-hat. + * + * @param xHat The state estimate x-hat. + */ + @SuppressWarnings("ParameterName") + @Override + public void setXhat(Matrix xHat) { + m_xHat = xHat; + } + + /** + * Set an element of the initial state estimate x-hat. + * + * @param row Row of x-hat. + * @param value Value for element of x-hat. + */ + @Override + public void setXhat(int row, double value) { + m_xHat.set(row, 0, value); + } + + /** + * Resets the observer. + */ + @Override + public void reset() { + m_xHat = new Matrix<>(m_states, Nat.N1()); + m_P = new Matrix<>(m_states, m_states); + m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1)); + } + + /** + * Project the model into the future with a new control input u. + * + * @param u New control input from controller. + * @param dtSeconds Timestep for prediction. + */ + @SuppressWarnings({"LocalVariableName", "ParameterName"}) + @Override + public void predict(Matrix u, double dtSeconds) { + // Discretize Q before projecting mean and covariance forward + Matrix contA = + NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u); + var discQ = + Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds).getSecond(); + + var sigmas = m_pts.sigmaPoints(m_xHat, m_P); + + for (int i = 0; i < m_pts.getNumSigmas(); ++i) { + Matrix x = sigmas.extractColumnVector(i); + + m_sigmasF.setColumn(i, RungeKutta.rungeKutta(m_f, x, u, dtSeconds)); + } + + var ret = unscentedTransform(m_states, m_states, + m_sigmasF, m_pts.getWm(), m_pts.getWc()); + + m_xHat = ret.getFirst(); + m_P = ret.getSecond().plus(discQ); + m_discR = Discretization.discretizeR(m_contR, dtSeconds); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * @param u Same control input used in the predict step. + * @param y Measurement vector. + */ + @SuppressWarnings("ParameterName") + @Override + public void correct(Matrix u, Matrix y) { + correct(m_outputs, u, y, m_h, m_discR); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + *

This is useful for when the measurements available during a timestep's + * Correct() call vary. The h(x, u) passed to the constructor is used if one + * is not provided (the two-argument version of this function). + * + * @param u Same control input used in the predict step. + * @param y Measurement vector. + * @param h A vector-valued function of x and u that returns + * the measurement vector. + * @param R Measurement noise covariance matrix. + */ + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + public void correct( + Nat rows, Matrix u, + Matrix y, + BiFunction, Matrix, Matrix> h, + Matrix R) { + // Transform sigma points into measurement space + Matrix sigmasH = new Matrix<>(new SimpleMatrix( + rows.getNum(), 2 * m_states.getNum() + 1)); + var sigmas = m_pts.sigmaPoints(m_xHat, m_P); + for (int i = 0; i < m_pts.getNumSigmas(); i++) { + Matrix hRet = h.apply( + sigmas.extractColumnVector(i), + u + ); + sigmasH.setColumn(i, hRet); + } + + // Mean and covariance of prediction passed through unscented transform + var transRet = unscentedTransform(m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc()); + var yHat = transRet.getFirst(); + var Py = transRet.getSecond().plus(R); + + // Compute cross covariance of the state and the measurements + Matrix Pxy = new Matrix<>(m_states, rows); + for (int i = 0; i < m_pts.getNumSigmas(); i++) { + var temp = + m_sigmasF.extractColumnVector(i).minus(m_xHat) + .times(sigmasH.extractColumnVector(i).minus(yHat).transpose()); + + Pxy = Pxy.plus(temp.times(m_pts.getWc(i))); + } + + // K = P_{xy} Py^-1 + // K^T = P_y^T^-1 P_{xy}^T + // P_y^T K^T = P_{xy}^T + // K^T = P_y^T.solve(P_{xy}^T) + // K = (P_y^T.solve(P_{xy}^T)^T + Matrix K = new Matrix<>( + Py.transpose().solve(Pxy.transpose()).transpose() + ); + + m_xHat = m_xHat.plus(K.times(y.minus(yHat))); + m_P = m_P.minus(K.times(Py).times(K.transpose())); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/math/Discretization.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/math/Discretization.java new file mode 100644 index 0000000000..1c3ee1cbdc --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/math/Discretization.java @@ -0,0 +1,179 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.math; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.Pair; + +@SuppressWarnings({"PMD.TooManyMethods", "ParameterName", "MethodTypeParameterName"}) +public final class Discretization { + private Discretization() { + // Utility class + } + + /** + * Discretizes the given continuous A matrix. + * + * @param Num representing the number of states. + * @param contA Continuous system matrix. + * @param dtSeconds Discretization timestep. + * @return the discrete matrix system. + */ + public static Matrix discretizeA( + Matrix contA, double dtSeconds) { + return contA.times(dtSeconds).exp(); + } + + /** + * Discretizes the given continuous A and B matrices. + * + *

Rather than solving a (States + Inputs) x (States + Inputs) matrix + * exponential like in DiscretizeAB(), we take advantage of the structure of the + * block matrix of A and B. + * + *

1) The exponential of A*t, which is only N x N, is relatively cheap. + * 2) The upper-right quarter of the (States + Inputs) x (States + Inputs) + * matrix, which we can approximate using a taylor series to several terms + * and still be substantially cheaper than taking the big exponential. + * + * @param states Nat representing the states of the system. + * @param contA Continuous system matrix. + * @param contB Continuous input matrix. + * @param dtseconds Discretization timestep. + */ + public static Pair, + Matrix> + discretizeABTaylor(Nat states, + Matrix contA, + Matrix contB, + double dtseconds) { + Matrix lastTerm = Matrix.eye(states); + double lastCoeff = dtseconds; + + var phi12 = lastTerm.times(lastCoeff); + + // i = 6 i.e. 5th order should be enough precision + for (int i = 2; i < 6; ++i) { + lastTerm = contA.times(lastTerm); + lastCoeff *= dtseconds / ((double) i); + + phi12 = phi12.plus(lastTerm.times(lastCoeff)); + } + + var discB = phi12.times(contB); + + var discA = discretizeA(contA, dtseconds); + + return Pair.of(discA, discB); + } + + /** + * Discretizes the given continuous A and Q matrices. + * + *

Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which + * is expensive), we take advantage of the structure of the block matrix of A + * and Q. + * + *

The exponential of A*t, which is only N x N, is relatively cheap. + * 2) The upper-right quarter of the 2N x 2N matrix, which we can approximate + * using a taylor series to several terms and still be substantially cheaper + * than taking the big exponential. + * + * @param Nat representing the number of states. + * @param contA Continuous system matrix. + * @param contQ Continuous process noise covariance matrix. + * @param dtSeconds Discretization timestep. + * @return a pair representing the discrete system matrix and process noise covariance matrix. + */ + @SuppressWarnings("LocalVariableName") + public static Pair, + Matrix> discretizeAQTaylor(Matrix contA, + Matrix contQ, + double dtSeconds) { + Matrix Q = (contQ.plus(contQ.transpose())).div(2.0); + + + Matrix lastTerm = Q.copy(); + double lastCoeff = dtSeconds; + + // A^T^n + Matrix Atn = contA.transpose(); + Matrix phi12 = lastTerm.times(lastCoeff); + + // i = 6 i.e. 6th order should be enough precision + for (int i = 2; i < 6; ++i) { + lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(Atn)); + lastCoeff *= dtSeconds / ((double) i); + + phi12 = phi12.plus(lastTerm.times(lastCoeff)); + + Atn = Atn.times(contA.transpose()); + } + + var discA = discretizeA(contA, dtSeconds); + Q = discA.times(phi12); + + // Make Q symmetric if it isn't already + var discQ = Q.plus(Q.transpose()).div(2.0); + + return new Pair<>(discA, discQ); + } + + /** + * Returns a discretized version of the provided continuous measurement noise + * covariance matrix. Note that dt=0.0 divides R by zero. + * + * @param Nat representing the number of outputs. + * @param R Continuous measurement noise covariance matrix. + * @param dtSeconds Discretization timestep. + * @return Discretized version of the provided continuous measurement noise covariance matrix. + */ + public static Matrix discretizeR(Matrix R, double dtSeconds) { + return R.div(dtSeconds); + } + + /** + * Discretizes the given continuous A and B matrices. + * + * @param Nat representing the states of the system. + * @param Nat representing the inputs to the system. + * @param contA Continuous system matrix. + * @param contB Continuous input matrix. + * @param dtSeconds Discretization timestep. + * @return a Pair representing discA and diskB. + */ + @SuppressWarnings("LocalVariableName") + public static Pair, + Matrix> discretizeAB( + Matrix contA, + Matrix contB, + double dtSeconds) { + var scaledA = contA.times(dtSeconds); + var scaledB = contB.times(dtSeconds); + + var contSize = contB.getNumRows() + contB.getNumCols(); + var Mcont = new Matrix<>(new SimpleMatrix(contSize, contSize)); + Mcont.assignBlock(0, 0, scaledA); + Mcont.assignBlock(0, scaledA.getNumCols(), scaledB); + var Mdisc = Mcont.exp(); + + var discA = new Matrix(new SimpleMatrix(contB.getNumRows(), + contB.getNumRows())); + var discB = new Matrix(new SimpleMatrix(contB.getNumRows(), + contB.getNumCols())); + + discA.extractFrom(0, 0, Mdisc); + discB.extractFrom(0, contB.getNumRows(), Mdisc); + + return new Pair<>(discA, discB); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java new file mode 100644 index 0000000000..c37d9182c5 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java @@ -0,0 +1,180 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.math; + +import java.util.Random; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpiutil.math.MathUtil; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.WPIMathJNI; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N3; + +@SuppressWarnings({"PMD.TooManyMethods", "ParameterName"}) +public final class StateSpaceUtil { + private StateSpaceUtil() { + // Utility class + } + + /** + * Creates a covariance matrix from the given vector for use with Kalman + * filters. + * + *

Each element is squared and placed on the covariance matrix diagonal. + * + * @param Num representing the states of the system. + * @param states A Nat representing the states of the system. + * @param stdDevs For a Q matrix, its elements are the standard deviations of + * each state from how the model behaves. For an R matrix, its + * elements are the standard deviations for each output + * measurement. + * @return Process noise or measurement noise covariance matrix. + */ + @SuppressWarnings("MethodTypeParameterName") + public static Matrix makeCovarianceMatrix( + Nat states, Matrix stdDevs + ) { + var result = new Matrix<>(states, states); + for (int i = 0; i < states.getNum(); i++) { + result.set(i, i, Math.pow(stdDevs.get(i, 0), 2)); + } + return result; + } + + /** + * Creates a vector of normally distributed white noise with the given noise + * intensities for each element. + * + * @param Num representing the dimensionality of the noise vector to create. + * @param stdDevs A matrix whose elements are the standard deviations of each + * element of the noise vector. + * @return White noise vector. + */ + public static Matrix makeWhiteNoiseVector( + Matrix stdDevs + ) { + var rand = new Random(); + + Matrix result = new Matrix<>(new SimpleMatrix(stdDevs.getNumRows(), 1)); + for (int i = 0; i < stdDevs.getNumRows(); i++) { + result.set(i, 0, rand.nextGaussian() * stdDevs.get(i, 0)); + } + return result; + } + + /** + * Creates a cost matrix from the given vector for use with LQR. + * + *

The cost matrix is constructed using Bryson's rule. The inverse square of + * each element in the input is taken and placed on the cost matrix diagonal. + * + * @param Nat representing the states of the system. + * @param costs An array. For a Q matrix, its elements are the maximum allowed + * excursions of the states from the reference. For an R matrix, + * its elements are the maximum allowed excursions of the control + * inputs from no actuation. + * @return State excursion or control effort cost matrix. + */ + @SuppressWarnings("MethodTypeParameterName") + public static Matrix + makeCostMatrix(Matrix costs) { + Matrix result = + new Matrix<>(new SimpleMatrix(costs.getNumRows(), costs.getNumRows())); + result.fill(0.0); + + for (int i = 0; i < costs.getNumRows(); i++) { + result.set(i, i, 1.0 / (Math.pow(costs.get(i, 0), 2))); + } + + return result; + } + + /** + * Returns true if (A, B) is a stabilizable pair. + * + *

(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if + * any, have absolute values less than one, where an eigenvalue is + * uncontrollable if rank(lambda * I - A, B) %3C n where n is number of states. + * + * @param Num representing the size of A. + * @param Num representing the columns of B. + * @param A System matrix. + * @param B Input matrix. + * @return If the system is stabilizable. + */ + @SuppressWarnings("MethodTypeParameterName") + public static boolean isStabilizable( + Matrix A, Matrix B) { + return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(), + A.getData(), B.getData()); + } + + /** + * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians. + * + * @param pose A pose to convert to a vector. + * @return The given pose in vector form, with the third element, theta, in radians. + */ + public static Matrix poseToVector(Pose2d pose) { + return VecBuilder.fill( + pose.getX(), + pose.getY(), + pose.getRotation().getRadians() + ); + } + + /** + * Clamp the input u to the min and max. + * + * @param u The input to clamp. + * @param umin The minimum input magnitude. + * @param umax The maximum input magnitude. + * @param The number of inputs. + * @return The clamped input. + */ + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + public static Matrix clampInputMaxMagnitude(Matrix u, + Matrix umin, + Matrix umax) { + var result = new Matrix(new SimpleMatrix(u.getNumRows(), 1)); + for (int i = 0; i < u.getNumRows(); i++) { + result.set(i, 0, MathUtil.clamp( + u.get(i, 0), + umin.get(i, 0), + umax.get(i, 0))); + } + return result; + } + + /** + * Normalize all inputs if any excedes the maximum magnitude. Useful for systems such as + * differential drivetrains. + * + * @param u The input vector. + * @param maxMagnitude The maximum magnitude any input can have. + * @param The number of inputs. + * @return The normalizedInput + */ + public static Matrix normalizeInputVector(Matrix u, + double maxMagnitude) { + double maxValue = u.maxAbs(); + boolean isCapped = maxValue > maxMagnitude; + + if (isCapped) { + return u.times(maxMagnitude / maxValue); + } + return u; + } + +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystem.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystem.java new file mode 100644 index 0000000000..703133c237 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystem.java @@ -0,0 +1,182 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.system; + +import edu.wpi.first.wpilibj.math.Discretization; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; + +@SuppressWarnings({"PMD.TooManyMethods", "ClassTypeParameterName"}) +public class LinearSystem { + + /** + * Continuous system matrix. + */ + @SuppressWarnings("MemberName") + private final Matrix m_A; + + /** + * Continuous input matrix. + */ + @SuppressWarnings("MemberName") + private final Matrix m_B; + + /** + * Output matrix. + */ + @SuppressWarnings("MemberName") + private final Matrix m_C; + + /** + * Feedthrough matrix. + */ + @SuppressWarnings("MemberName") + private final Matrix m_D; + + /** + * Construct a new LinearSystem from the four system matrices. + * + * @param a The system matrix A. + * @param b The input matrix B. + * @param c The output matrix C. + * @param d The feedthrough matrix D. + */ + @SuppressWarnings("ParameterName") + public LinearSystem(Matrix a, Matrix b, + Matrix c, Matrix d) { + + this.m_A = a; + this.m_B = b; + this.m_C = c; + this.m_D = d; + } + + /** + * Returns the system matrix A. + * + * @return the system matrix A. + */ + public Matrix getA() { + return m_A; + } + + /** + * Returns an element of the system matrix A. + * + * @param row Row of A. + * @param col Column of A. + * @return the system matrix A at (i, j). + */ + public double getA(int row, int col) { + return m_A.get(row, col); + } + + /** + * Returns the input matrix B. + * + * @return the input matrix B. + */ + public Matrix getB() { + return m_B; + } + + /** + * Returns an element of the input matrix B. + * + * @param row Row of B. + * @param col Column of B. + * @return The value of the input matrix B at (i, j). + */ + public double getB(int row, int col) { + return m_B.get(row, col); + } + + /** + * Returns the output matrix C. + * + * @return Output matrix C. + */ + public Matrix getC() { + return m_C; + } + + /** + * Returns an element of the output matrix C. + * + * @param row Row of C. + * @param col Column of C. + * @return the double value of C at the given position. + */ + public double getC(int row, int col) { + return m_C.get(row, col); + } + + /** + * Returns the feedthrough matrix D. + * + * @return the feedthrough matrix D. + */ + public Matrix getD() { + return m_D; + } + + /** + * Returns an element of the feedthrough matrix D. + * + * @param row Row of D. + * @param col Column of D. + * @return The feedthrough matrix D at (i, j). + */ + public double getD(int row, int col) { + return m_D.get(row, col); + } + + /** + * Computes the new x given the old x and the control input. + * + *

This is used by state observers directly to run updates based on state + * estimate. + * + * @param x The current state. + * @param clampedU The control input. + * @param dtSeconds Timestep for model update. + * @return the updated x. + */ + @SuppressWarnings("ParameterName") + public Matrix calculateX(Matrix x, Matrix clampedU, + double dtSeconds) { + var discABpair = Discretization.discretizeAB(m_A, m_B, dtSeconds); + + return (discABpair.getFirst().times(x)).plus(discABpair.getSecond().times(clampedU)); + } + + /** + * Computes the new y given the control input. + * + *

This is used by state observers directly to run updates based on state + * estimate. + * + * @param x The current state. + * @param clampedU The control input. + * @return the updated output matrix Y. + */ + @SuppressWarnings("ParameterName") + public Matrix calculateY( + Matrix x, + Matrix clampedU) { + return m_C.times(x).plus(m_D.times(clampedU)); + } + + @Override + public String toString() { + return String.format("Linear System: A\n%s\n\nB:\n%s\n\nC:\n%s\n\nD:\n%s\n", m_A.toString(), + m_B.toString(), m_C.toString(), m_D.toString()); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java new file mode 100644 index 0000000000..bc4f36d129 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java @@ -0,0 +1,357 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.system; + +import java.util.function.Function; + +import org.ejml.MatrixDimensionException; +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.wpilibj.controller.LinearPlantInversionFeedforward; +import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator; +import edu.wpi.first.wpilibj.estimator.KalmanFilter; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; + +/** + * Combines a plant, controller, and observer for controlling a mechanism with + * full state feedback. + * + *

For everything in this file, "inputs" and "outputs" are defined from the + * perspective of the plant. This means U is an input and Y is an output + * (because you give the plant U (powers) and it gives you back a Y (sensor + * values). This is the opposite of what they mean from the perspective of the + * controller (U is an output because that's what goes to the motors and Y is an + * input because that's what comes back from the sensors). + * + *

For more on the underlying math, read + * https://file.tavsys.net/control/state-space-guide.pdf. + */ +@SuppressWarnings("ClassTypeParameterName") +public class LinearSystemLoop { + + private final LinearSystem m_plant; + private final LinearQuadraticRegulator m_controller; + private final LinearPlantInversionFeedforward m_feedforward; + private final KalmanFilter m_observer; + private Matrix m_nextR; + private Function, Matrix> m_clampFunction; + + /** + * Constructs a state-space loop with the given plant, controller, and + * observer. By default, the initial reference is all zeros. Users should + * call reset with the initial system state before enabling the loop. This + * constructor assumes that the input(s) to this system are voltage. + * + * @param plant State-space plant. + * @param controller State-space controller. + * @param observer State-space observer. + * @param maxVoltageVolts The maximum voltage that can be applied. Commonly 12. + * @param dtSeconds The nominal timestep. + */ + public LinearSystemLoop(LinearSystem plant, + LinearQuadraticRegulator controller, + KalmanFilter observer, + double maxVoltageVolts, + double dtSeconds) { + this(plant, controller, + new LinearPlantInversionFeedforward<>(plant, dtSeconds), observer, + u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts)); + } + + /** + * Constructs a state-space loop with the given plant, controller, and + * observer. By default, the initial reference is all zeros. Users should + * call reset with the initial system state before enabling the loop. + * + * @param plant State-space plant. + * @param controller State-space controller. + * @param observer State-space observer. + * @param clampFunction The function used to clamp the input U. + * @param dtSeconds The nominal timestep. + */ + public LinearSystemLoop(LinearSystem plant, + LinearQuadraticRegulator controller, + KalmanFilter observer, + Function, Matrix> clampFunction, + double dtSeconds) { + this(plant, controller, new LinearPlantInversionFeedforward<>(plant, dtSeconds), + observer, clampFunction); + } + + /** + * Constructs a state-space loop with the given plant, controller, and + * observer. By default, the initial reference is all zeros. Users should + * call reset with the initial system state before enabling the loop. + * + * @param plant State-space plant. + * @param controller State-space controller. + * @param feedforward Plant inversion feedforward. + * @param observer State-space observer. + * @param maxVoltageVolts The maximum voltage that can be applied. Assumes that the + * inputs are voltages. + */ + public LinearSystemLoop(LinearSystem plant, + LinearQuadraticRegulator controller, + LinearPlantInversionFeedforward feedforward, + KalmanFilter observer, + double maxVoltageVolts + ) { + this(plant, controller, feedforward, + observer, u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts)); + } + + /** + * Constructs a state-space loop with the given plant, controller, and + * observer. By default, the initial reference is all zeros. Users should + * call reset with the initial system state before enabling the loop. + * + * @param plant State-space plant. + * @param controller State-space controller. + * @param feedforward Plant inversion feedforward. + * @param observer State-space observer. + * @param clampFunction The function used to clamp the input U. + */ + public LinearSystemLoop(LinearSystem plant, + LinearQuadraticRegulator controller, + LinearPlantInversionFeedforward feedforward, + KalmanFilter observer, + Function, Matrix> clampFunction) { + this.m_plant = plant; + this.m_controller = controller; + this.m_feedforward = feedforward; + this.m_observer = observer; + this.m_clampFunction = clampFunction; + + m_nextR = new Matrix<>(new SimpleMatrix(controller.getK().getNumCols(), 0)); + reset(m_nextR); + } + + /** + * Returns the observer's state estimate x-hat. + * + * @return the observer's state estimate x-hat. + */ + public Matrix getXHat() { + return getObserver().getXhat(); + } + + /** + * Returns an element of the observer's state estimate x-hat. + * + * @param row Row of x-hat. + * @return the i-th element of the observer's state estimate x-hat. + */ + public double getXHat(int row) { + return getObserver().getXhat(row); + } + + /** + * Set the initial state estimate x-hat. + * + * @param xhat The initial state estimate x-hat. + */ + public void setXHat(Matrix xhat) { + getObserver().setXhat(xhat); + } + + /** + * Set an element of the initial state estimate x-hat. + * + * @param row Row of x-hat. + * @param value Value for element of x-hat. + */ + public void setXHat(int row, double value) { + getObserver().setXhat(row, value); + } + + /** + * Returns an element of the controller's next reference r. + * + * @param row Row of r. + * @return the element i of the controller's next reference r. + */ + public double getNextR(int row) { + return getNextR().get(row, 0); + } + + /** + * Returns the controller's next reference r. + * + * @return the controller's next reference r. + */ + public Matrix getNextR() { + return m_nextR; + } + + /** + * Set the next reference r. + * + * @param nextR Next reference. + */ + public void setNextR(Matrix nextR) { + m_nextR = nextR; + } + + /** + * Set the next reference r. + * + * @param nextR Next reference. + */ + public void setNextR(double... nextR) { + if (nextR.length != m_nextR.getNumRows()) { + throw new MatrixDimensionException(String.format("The next reference does not have the " + + "correct number of entries! Expected %s, but got %s.", + m_nextR.getNumRows(), + nextR.length)); + } + m_nextR = new Matrix<>(new SimpleMatrix(m_nextR.getNumRows(), 1, true, nextR)); + } + + /** + * Returns the controller's calculated control input u plus the calculated feedforward u_ff. + * + * @return the calculated control input u. + */ + public Matrix getU() { + return clampInput(m_controller.getU().plus(m_feedforward.getUff())); + } + + /** + * Returns an element of the controller's calculated control input u. + * + * @param row Row of u. + * @return the calculated control input u at the row i. + */ + public double getU(int row) { + return getU().get(row, 0); + } + + /** + * Return the plant used internally. + * + * @return the plant used internally. + */ + public LinearSystem getPlant() { + return m_plant; + } + + /** + * Return the controller used internally. + * + * @return the controller used internally. + */ + public LinearQuadraticRegulator getController() { + return m_controller; + } + + /** + * Return the feedforward used internally. + * + * @return the feedforward used internally. + */ + public LinearPlantInversionFeedforward getFeedforward() { + return m_feedforward; + } + + /** + * Return the observer used internally. + * + * @return the observer used internally. + */ + public KalmanFilter getObserver() { + return m_observer; + } + + /** + * Zeroes reference r, controller output u and plant output y. + * The previous reference for PlantInversionFeedforward is set to the + * initial reference. + * @param initialReference The initial reference. + */ + public void reset(Matrix initialReference) { + m_controller.reset(); + m_feedforward.reset(initialReference); + m_observer.reset(); + m_nextR.fill(0.0); + } + + /** + * Returns difference between reoid predict(double dtSference r and x-hat. + * + * @return the + */ + public Matrix getError() { + return getController().getR().minus(m_observer.getXhat()); + } + + /** + * Returns difference between reference r and x-hat. + * + * @param index The index of the error matrix to return. + * @return The error at that index. + */ + public double getError(int index) { + return (getController().getR().minus(m_observer.getXhat())).get(index, 0); + } + + /** + * Get the function used to clamp the input u. + * @return The clamping function. + */ + public Function, Matrix> getClampFunction() { + return m_clampFunction; + } + + /** + * Set the clamping function used to clamp inputs. + */ + public void setClampFunction(Function, Matrix> clampFunction) { + this.m_clampFunction = clampFunction; + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * @param y Measurement vector. + */ + @SuppressWarnings("ParameterName") + public void correct(Matrix y) { + getObserver().correct(getU(), y); + } + + /** + * Sets new controller output, projects model forward, and runs observer + * prediction. + * + *

After calling this, the user should send the elements of u to the + * actuators. + * + * @param dtSeconds Timestep for model update. + */ + @SuppressWarnings("LocalVariableName") + public void predict(double dtSeconds) { + var u = clampInput(m_controller.calculate(getObserver().getXhat(), m_nextR) + .plus(m_feedforward.calculate(m_nextR))); + getObserver().predict(u, dtSeconds); + } + + /** + * Clamp the input u to the min and max. + * + * @param unclampedU The input to clamp. + * @return The clamped input. + */ + public Matrix clampInput(Matrix unclampedU) { + return m_clampFunction.apply(unclampedU); + } + +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalJacobian.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalJacobian.java new file mode 100644 index 0000000000..a808dec6d2 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalJacobian.java @@ -0,0 +1,111 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.system; + +import java.util.function.BiFunction; +import java.util.function.Function; + +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; + +public final class NumericalJacobian { + private NumericalJacobian() { + // Utility Class. + } + + private static final double kEpsilon = 1e-5; + + /** + * Computes the numerical Jacobian with respect to x for f(x). + * + * @param Number of rows in the result of f(x). + * @param Num representing the number of rows in the output of f. + * @param Number of columns in the result of f(x). + * @param rows Number of rows in the result of f(x). + * @param cols Number of columns in the result of f(x). + * @param f Vector-valued function from which to compute the Jacobian. + * @param x Vector argument. + * @return The numerical Jacobian with respect to x for f(x, u, ...). + */ + @SuppressWarnings({"ParameterName", "MethodTypeParameterName"}) + public static Matrix + numericalJacobian( + Nat rows, + Nat cols, + Function, Matrix> f, + Matrix x + ) { + var result = new Matrix<>(rows, cols); + + for (int i = 0; i < cols.getNum(); i++) { + var dxPlus = x.copy(); + var dxMinus = x.copy(); + dxPlus.set(i, 0, dxPlus.get(i, 0) + kEpsilon); + dxMinus.set(i, 0, dxMinus.get(i, 0) - kEpsilon); + @SuppressWarnings("LocalVariableName") + var dF = f.apply(dxPlus).minus(f.apply(dxMinus)).div(2 * kEpsilon); + + result.setColumn(i, Matrix.changeBoundsUnchecked(dF)); + } + + return result; + } + + /** + * Returns numerical Jacobian with respect to x for f(x, u, ...). + * + * @param Number of rows in the result of f(x, u). + * @param Number of rows in x. + * @param Number of rows in the second input to f. + * @param Num representing the rows in the output of f. + * @param rows Number of rows in the result of f(x, u). + * @param states Number of rows in x. + * @param f Vector-valued function from which to compute Jacobian. + * @param x State vector. + * @param u Input vector. + * @return The numerical Jacobian with respect to x for f(x, u, ...). + */ + @SuppressWarnings({"ParameterName", "MethodTypeParameterName"}) + public static + Matrix numericalJacobianX( + Nat rows, + Nat states, + BiFunction, Matrix, Matrix> f, + Matrix x, + Matrix u + ) { + return numericalJacobian(rows, states, _x -> f.apply(_x, u), x); + } + + /** + * Returns the numerical Jacobian with respect to u for f(x, u). + * + * @param The states of the system. + * @param The inputs to the system. + * @param Number of rows in the result of f(x, u). + * @param rows Number of rows in the result of f(x, u). + * @param inputs Number of rows in u. + * @param f Vector-valued function from which to compute the Jacobian. + * @param x State vector. + * @param u Input vector. + * @return the numerical Jacobian with respect to u for f(x, u). + */ + @SuppressWarnings({"ParameterName", "MethodTypeParameterName"}) + public static Matrix + numericalJacobianU( + Nat rows, + Nat inputs, + BiFunction, Matrix, Matrix> f, + Matrix x, + Matrix u + ) { + return numericalJacobian(rows, inputs, _u -> f.apply(x, _u), u); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java new file mode 100644 index 0000000000..fef5ddff18 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java @@ -0,0 +1,113 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.system; + +import java.util.function.BiFunction; +import java.util.function.DoubleFunction; +import java.util.function.Function; + +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Num; +import edu.wpi.first.wpiutil.math.numbers.N1; + +public final class RungeKutta { + private RungeKutta() { + // utility Class + } + + /** + * Performs Runge Kutta integration (4th order). + * + * @param f The function to integrate, which takes one argument x. + * @param x The initial value of x. + * @param dtSeconds The time over which to integrate. + * @return the integration of dx/dt = f(x) for dt. + */ + @SuppressWarnings("ParameterName") + public static double rungeKutta( + DoubleFunction f, + double x, + double dtSeconds + ) { + final var halfDt = 0.5 * dtSeconds; + final var k1 = f.apply(x); + final var k2 = f.apply(x + k1 * halfDt); + final var k3 = f.apply(x + k2 * halfDt); + final var k4 = f.apply(x + k3 * dtSeconds); + return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4); + } + + /** + * Performs Runge Kutta integration (4th order). + * + * @param f The function to integrate. It must take two arguments x and u. + * @param x The initial value of x. + * @param u The value u held constant over the integration period. + * @param dtSeconds The time over which to integrate. + * @return The result of Runge Kutta integration (4th order). + */ + @SuppressWarnings("ParameterName") + public static double rungeKutta( + BiFunction f, + double x, Double u, double dtSeconds + ) { + final var halfDt = 0.5 * dtSeconds; + final var k1 = f.apply(x, u); + final var k2 = f.apply(x + k1 * halfDt, u); + final var k3 = f.apply(x + k2 * halfDt, u); + final var k4 = f.apply(x + k3 * dtSeconds, u); + return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4); + } + + /** + * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt. + * + * @param A Num representing the states of the system to integrate. + * @param A Num representing the inputs of the system to integrate. + * @param f The function to integrate. It must take two arguments x and u. + * @param x The initial value of x. + * @param u The value u held constant over the integration period. + * @param dtSeconds The time over which to integrate. + * @return the integration of dx/dt = f(x, u) for dt. + */ + @SuppressWarnings({"ParameterName", "MethodTypeParameterName"}) + public static Matrix rungeKutta( + BiFunction, Matrix, Matrix> f, + Matrix x, Matrix u, double dtSeconds) { + + final var halfDt = 0.5 * dtSeconds; + Matrix k1 = f.apply(x, u); + Matrix k2 = f.apply(x.plus(k1.times(halfDt)), u); + Matrix k3 = f.apply(x.plus(k2.times(halfDt)), u); + Matrix k4 = f.apply(x.plus(k3.times(dtSeconds)), u); + return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0)); + } + + /** + * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt. + * + * @param A Num prepresenting the states of the system. + * @param f The function to integrate. It must take one argument x. + * @param x The initial value of x. + * @param dtSeconds The time over which to integrate. + * @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt. + */ + @SuppressWarnings({"ParameterName", "MethodTypeParameterName"}) + public static Matrix rungeKutta( + Function, Matrix> f, + Matrix x, double dtSeconds) { + + final var halfDt = 0.5 * dtSeconds; + Matrix k1 = f.apply(x); + Matrix k2 = f.apply(x.plus(k1.times(halfDt))); + Matrix k3 = f.apply(x.plus(k2.times(halfDt))); + Matrix k4 = f.apply(x.plus(k3.times(dtSeconds))); + return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0)); + } + +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java new file mode 100644 index 0000000000..a38629f930 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java @@ -0,0 +1,171 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.system.plant; + +import edu.wpi.first.wpilibj.util.Units; + +/** + * Holds the constants for a DC motor. + */ +public class DCMotor { + public final double m_nominalVoltageVolts; + public final double m_stallTorqueNewtonMeters; + public final double m_stallCurrentAmps; + public final double m_freeCurrentAmps; + public final double m_freeSpeedRadPerSec; + @SuppressWarnings("MemberName") + public final double m_rOhms; + @SuppressWarnings("MemberName") + public final double m_KvRadPerSecPerVolt; + @SuppressWarnings("MemberName") + public final double m_KtNMPerAmp; + + + /** + * Constructs a DC motor. + * + * @param nominalVoltageVolts Voltage at which the motor constants were measured. + * @param stallTorqueNewtonMeters Current draw when stalled. + * @param stallCurrentAmps Current draw when stalled. + * @param freeCurrentAmps Current draw under no load. + * @param freeSpeedRadPerSec Angular velocity under no load. + */ + public DCMotor(double nominalVoltageVolts, + double stallTorqueNewtonMeters, + double stallCurrentAmps, + double freeCurrentAmps, + double freeSpeedRadPerSec) { + this.m_nominalVoltageVolts = nominalVoltageVolts; + this.m_stallTorqueNewtonMeters = stallTorqueNewtonMeters; + this.m_stallCurrentAmps = stallCurrentAmps; + this.m_freeCurrentAmps = freeCurrentAmps; + this.m_freeSpeedRadPerSec = freeSpeedRadPerSec; + + this.m_rOhms = nominalVoltageVolts / stallCurrentAmps; + this.m_KvRadPerSecPerVolt = freeSpeedRadPerSec / (nominalVoltageVolts - m_rOhms + * freeCurrentAmps); + this.m_KtNMPerAmp = stallTorqueNewtonMeters / stallCurrentAmps; + } + + /** + * Return a gearbox of CIM motors. + * + * @param numMotors Number of motors in the gearbox. + */ + public static DCMotor getCIM(int numMotors) { + return new DCMotor(12, + 2.42 * numMotors, 133, + 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310)); + } + + /** + * Return a gearbox of 775Pro motors. + * + * @param numMotors Number of motors in the gearbox. + */ + public static DCMotor getVex775Pro(int numMotors) { + return gearbox(new DCMotor(12, + 0.71, 134, + 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730)), numMotors); + } + + /** + * Return a gearbox of NEO motors. + * + * @param numMotors Number of motors in the gearbox. + */ + public static DCMotor getNEO(int numMotors) { + return gearbox(new DCMotor(12, 2.6, + 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676)), numMotors); + } + + /** + * Return a gearbox of MiniCIM motors. + * + * @param numMotors Number of motors in the gearbox. + */ + public static DCMotor getMiniCIM(int numMotors) { + return gearbox(new DCMotor(12, 1.41, 89, 3, + Units.rotationsPerMinuteToRadiansPerSecond(5840)), numMotors); + } + + /** + * Return a gearbox of Bag motors. + * + * @param numMotors Number of motors in the gearbox. + */ + public static DCMotor getBag(int numMotors) { + return gearbox(new DCMotor(12, 0.43, 53, 1.8, + Units.rotationsPerMinuteToRadiansPerSecond(13180)), numMotors); + } + + /** + * Return a gearbox of Andymark RS775-125 motors. + * + * @param numMotors Number of motors in the gearbox. + */ + public static DCMotor getAndymarkRs775_125(int numMotors) { + return gearbox(new DCMotor(12, 0.28, 18, 1.6, + Units.rotationsPerMinuteToRadiansPerSecond(5800.0)), numMotors); + } + + /** + * Return a gearbox of Banebots RS775 motors. + * + * @param numMotors Number of motors in the gearbox. + */ + public static DCMotor getBanebotsRs775(int numMotors) { + return gearbox(new DCMotor(12, 0.72, 97, 2.7, + Units.rotationsPerMinuteToRadiansPerSecond(13050.0)), numMotors); + } + + /** + * Return a gearbox of Andymark 9015 motors. + * + * @param numMotors Number of motors in the gearbox. + */ + public static DCMotor getAndymark9015(int numMotors) { + return gearbox(new DCMotor(12, 0.36, 71, 3.7, + Units.rotationsPerMinuteToRadiansPerSecond(14270.0)), numMotors); + } + + /** + * Return a gearbox of Banebots RS 550 motors. + * + * @param numMotors Number of motors in the gearbox. + */ + public static DCMotor getBanebotsRs550(int numMotors) { + return gearbox(new DCMotor(12, 0.38, 84, 0.4, + Units.rotationsPerMinuteToRadiansPerSecond(19000.0)), numMotors); + } + + /** + * Return a gearbox of Neo 550 motors. + * + * @param numMotors Number of motors in the gearbox. + */ + public static DCMotor getNeo550(int numMotors) { + return gearbox(new DCMotor(12, 0.97, 100, 1.4, + Units.rotationsPerMinuteToRadiansPerSecond(11000.0)), numMotors); + } + + /** + * Return a gearbox of Falcon 500 motors. + * + * @param numMotors Number of motors in the gearbox. + */ + public static DCMotor getFalcon500(int numMotors) { + return gearbox(new DCMotor(12, 4.69, 257, 1.5, + Units.rotationsPerMinuteToRadiansPerSecond(6380.0)), numMotors); + } + + private static DCMotor gearbox(DCMotor motor, double numMotors) { + return new DCMotor(motor.m_nominalVoltageVolts, motor.m_stallTorqueNewtonMeters * numMotors, + motor.m_stallCurrentAmps, motor.m_freeCurrentAmps, motor.m_freeSpeedRadPerSec); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java new file mode 100644 index 0000000000..ebd7782b7f --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java @@ -0,0 +1,199 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.system.plant; + +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; + +public final class LinearSystemId { + private LinearSystemId() { + // Utility class + } + + /** + * Create a state-space model of an elevator system. + * + * @param motor The motor (or gearbox) attached to the arm. + * @param massKg The mass of the elevator carriage, in kilograms. + * @param radiusMeters The radius of thd driving drum of the elevator, in meters. + * @param G The reduction between motor and drum, as a ratio of output to input. + * @return A LinearSystem representing the given characterized constants. + */ + @SuppressWarnings("ParameterName") + public static LinearSystem createElevatorSystem(DCMotor motor, double massKg, + double radiusMeters, double G) { + return new LinearSystem<>( + Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, + 0, -Math.pow(G, 2) * motor.m_KtNMPerAmp + / (motor.m_rOhms * radiusMeters * radiusMeters * massKg + * motor.m_KvRadPerSecPerVolt)), + VecBuilder.fill( + 0, G * motor.m_KtNMPerAmp / (motor.m_rOhms * radiusMeters * massKg)), + Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), + new Matrix<>(Nat.N1(), Nat.N1())); + } + + /** + * Create a state-space model of a flywheel system. + * + * @param motor The motor (or gearbox) attached to the arm. + * @param jKgMetersSquared The moment of inertia J of the flywheel. + * @param G The reduction between motor and drum, as a ratio of output to input. + * @return A LinearSystem representing the given characterized constants. + */ + @SuppressWarnings("ParameterName") + public static LinearSystem createFlywheelSystem(DCMotor motor, + double jKgMetersSquared, + double G) { + return new LinearSystem<>( + VecBuilder.fill( + -G * G * motor.m_KtNMPerAmp + / (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * jKgMetersSquared)), + VecBuilder.fill(G * motor.m_KtNMPerAmp + / (motor.m_rOhms * jKgMetersSquared)), + Matrix.eye(Nat.N1()), + new Matrix<>(Nat.N1(), Nat.N1())); + } + + /** + * Create a state-space model of a differential drive drivetrain. + * + * @param motor the gearbox representing the motors driving the drivetrain. + * @param massKg the mass of the robot. + * @param rMeters the radius of the wheels in meters. + * @param rbMeters the radius of the base (half the track width) in meters. + * @param JKgMetersSquared the moment of inertia of the robot. + * @param G the gearing reduction as output over input. + * @return A LinearSystem representing a differential drivetrain. + */ + @SuppressWarnings({"LocalVariableName", "ParameterName"}) + public static LinearSystem createDrivetrainVelocitySystem(DCMotor motor, + double massKg, + double rMeters, + double rbMeters, + double JKgMetersSquared, + double G) { + var C1 = + -(G * G) * motor.m_KtNMPerAmp + / (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * rMeters * rMeters); + var C2 = G * motor.m_KtNMPerAmp / (motor.m_rOhms * rMeters); + + final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared; + final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared; + var A = Matrix.mat(Nat.N2(), Nat.N2()).fill( + C3 * C1, + C4 * C1, + C4 * C1, + C3 * C1); + var B = Matrix.mat(Nat.N2(), Nat.N2()).fill( + C3 * C2, + C4 * C2, + C4 * C2, + C3 * C2); + var C = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0); + var D = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0); + + return new LinearSystem<>(A, B, C, D); + } + + /** + * Create a state-space model of a single jointed arm system. + * + * @param motor The motor (or gearbox) attached to the arm. + * @param jKgSquaredMeters The moment of inertia J of the arm. + * @param G the gearing between the motor and arm, in output over input. + * Most of the time this will be greater than 1. + * @return A LinearSystem representing the given characterized constants. + */ + @SuppressWarnings("ParameterName") + public static LinearSystem createSingleJointedArmSystem(DCMotor motor, + double jKgSquaredMeters, + double G) { + return new LinearSystem<>( + Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, + 0, -Math.pow(G, 2) * motor.m_KtNMPerAmp + / (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * jKgSquaredMeters)), + VecBuilder.fill(0, G * motor.m_KtNMPerAmp + / (motor.m_rOhms * jKgSquaredMeters)), + Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), + new Matrix<>(Nat.N1(), Nat.N1())); + } + + /** + * Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). + * These constants cam be found using frc-characterization. + * + * @param kV The velocity gain, in volts per (units per second) + * @param kA The acceleration gain, in volts per (units per second squared) + * @return A LinearSystem representing the given characterized constants. + * @see + * https://github.com/wpilibsuite/frc-characterization + */ + @SuppressWarnings("ParameterName") + public static LinearSystem identifyVelocitySystem(double kV, double kA) { + return new LinearSystem<>( + VecBuilder.fill(-kV / kA), + VecBuilder.fill(1.0 / kA), + VecBuilder.fill(1.0), + VecBuilder.fill(0.0)); + } + + /** + * Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). + * These constants cam be found using frc-characterization. + * + * @param kV The velocity gain, in volts per (units per second) + * @param kA The acceleration gain, in volts per (units per second squared) + * @return A LinearSystem representing the given characterized constants. + * @see + * https://github.com/wpilibsuite/frc-characterization + */ + @SuppressWarnings("ParameterName") + public static LinearSystem identifyPositionSystem(double kV, double kA) { + return new LinearSystem<>( + Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA), + VecBuilder.fill(0.0, 1.0 / kA), + Matrix.mat(Nat.N1(), Nat.N2()).fill(1.0, 0.0), + VecBuilder.fill(0.0)); + } + + /** + * Identify a standard differential drive drivetrain, given the drivetrain's + * kV and kA in both linear (volts/(meter/sec) and volts/(meter/sec^2)) and + * angular (volts/(radian/sec) and volts/(radian/sec^2)) cases. This can be + * found using frc-characterization. + * + * @param kVLinear The linear velocity gain, volts per (meter per second). + * @param kALinear The linear acceleration gain, volts per (meter per second squared). + * @param kVAngular The angular velocity gain, volts per (radians per second). + * @param kAAngular The angular acceleration gain, volts per (radians per second squared). + * @return A LinearSystem representing the given characterized constants. + * @see + * https://github.com/wpilibsuite/frc-characterization + */ + @SuppressWarnings("ParameterName") + public static LinearSystem identifyDrivetrainSystem( + double kVLinear, double kALinear, double kVAngular, double kAAngular) { + + final double c = 0.5 / (kALinear * kAAngular); + final double A1 = c * (-kALinear * kVAngular - kVLinear * kAAngular); + final double A2 = c * (kALinear * kVAngular - kVLinear * kAAngular); + final double B1 = c * (kALinear + kAAngular); + final double B2 = c * (kAAngular - kALinear); + + return new LinearSystem<>( + Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1), + Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1), + Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1), + Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0)); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java index 7db7454bf9..d8b20a3784 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java @@ -18,8 +18,8 @@ import org.ejml.simple.SimpleMatrix; * @param The number of columns of the desired matrix. */ public class MatBuilder { - private final Nat m_rows; - private final Nat m_cols; + final Nat m_rows; + final Nat m_cols; /** * Fills the matrix with the given data, encoded in row major form. diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java index ffb165c22b..face6ea924 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java @@ -9,10 +9,17 @@ package edu.wpi.first.wpiutil.math; import java.util.Objects; +import org.ejml.MatrixDimensionException; +import org.ejml.data.DMatrixRMaj; import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.dense.row.MatrixFeatures_DDRM; import org.ejml.dense.row.NormOps_DDRM; +import org.ejml.dense.row.factory.DecompositionFactory_DDRM; +import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64; import org.ejml.simple.SimpleMatrix; +import edu.wpi.first.wpiutil.math.numbers.N1; + /** * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices. * @@ -21,10 +28,60 @@ import org.ejml.simple.SimpleMatrix; * @param The number of rows in this matrix. * @param The number of columns in this matrix. */ -@SuppressWarnings("PMD.TooManyMethods") +@SuppressWarnings({"PMD.TooManyMethods", "PMD.ExcessivePublicCount"}) public class Matrix { + protected final SimpleMatrix m_storage; - private final SimpleMatrix m_storage; + /** + * Constructs an empty zero matrix of the given dimensions. + * + * @param rows The number of rows of the matrix. + * @param columns The number of columns of the matrix. + */ + public Matrix(Nat rows, Nat columns) { + this.m_storage = new SimpleMatrix( + Objects.requireNonNull(rows).getNum(), + Objects.requireNonNull(columns).getNum() + ); + } + + /** + * Constructs a new {@link Matrix} with the given storage. + * Caller should make sure that the provided generic bounds match + * the shape of the provided {@link Matrix}. + * + *

NOTE:It is not recommend to use this constructor unless the + * {@link SimpleMatrix} API is absolutely necessary due to the desired + * function not being accessible through the {@link Matrix} wrapper. + * + * @param storage The {@link SimpleMatrix} to back this value. + */ + public Matrix(SimpleMatrix storage) { + this.m_storage = Objects.requireNonNull(storage); + } + + /** + * Constructs a new matrix with the storage of the supplied matrix. + * + * @param other The {@link Matrix} to copy the storage of. + */ + public Matrix(Matrix other) { + this.m_storage = Objects.requireNonNull(other).getStorage().copy(); + } + + /** + * Gets the underlying {@link SimpleMatrix} that this {@link Matrix} wraps. + * + *

NOTE:The use of this method is heavily discouraged as this removes any + * guarantee of type safety. This should only be called if the {@link SimpleMatrix} + * API is absolutely necessary due to the desired function not being accessible through + * the {@link Matrix} wrapper. + * + * @return The underlying {@link SimpleMatrix} storage. + */ + public SimpleMatrix getStorage() { + return m_storage; + } /** * Gets the number of columns in this matrix. @@ -58,8 +115,8 @@ public class Matrix { /** * Sets the value at the given indices. * - * @param row The row of the element. - * @param col The column of the element. + * @param row The row of the element. + * @param col The column of the element. * @param value The value to insert at the given location. */ public final void set(int row, int col, double value) { @@ -67,10 +124,44 @@ public class Matrix { } /** - * If a vector then a square matrix is returned - * if a matrix then a vector of diagonal elements is returned. + * Sets a row to a given row vector. * - * @return Diagonal elements inside a vector or a square matrix with the same diagonal elements. + * @param row The row to set. + * @param val The row vector to set the given row to. + */ + public final void setRow(int row, Matrix val) { + this.m_storage.setRow(row, 0, + Objects.requireNonNull(val).m_storage.getDDRM().getData()); + } + + /** + * Sets a column to a given column vector. + * + * @param column The column to set. + * @param val The column vector to set the given row to. + */ + public final void setColumn(int column, Matrix val) { + this.m_storage.setColumn(column, 0, + Objects.requireNonNull(val).m_storage.getDDRM().getData()); + } + + + /** + * Sets all the elements in "this" matrix equal to the specified value. + * + * @param value The value each element is set to. + */ + public void fill(double value) { + this.m_storage.fill(value); + } + + /** + * Returns the diagonal elements inside a vector or square matrix. + * + *

If "this" {@link Matrix} is a vector then a square matrix is returned. If a "this" + * {@link Matrix} is a matrix then a vector of diagonal elements is returned. + * + * @return The diagonal elements inside a vector or a square matrix. */ public final Matrix diag() { return new Matrix<>(this.m_storage.diag()); @@ -81,10 +172,20 @@ public class Matrix { * * @return The largest element of this matrix. */ - public final double maxInternal() { + public final double max() { return CommonOps_DDRM.elementMax(this.m_storage.getDDRM()); } + /** + * Returns the absolute value of the element in this matrix with the largest absolute value. + * + * @return The absolute value of the element with the largest absolute value. + */ + public final double maxAbs() { + return CommonOps_DDRM.elementMaxAbs(this.m_storage.getDDRM()); + } + + /** * Returns the smallest element of this matrix. * @@ -112,10 +213,10 @@ public class Matrix { * * @param other The other matrix to multiply by. * @param The number of columns in the second matrix. - * @return The result of the matrix multiplication between this and the given matrix. + * @return The result of the matrix multiplication between "this" and the given matrix. */ public final Matrix times(Matrix other) { - return new Matrix<>(this.m_storage.mult(other.m_storage)); + return new Matrix<>(this.m_storage.mult(Objects.requireNonNull(other).m_storage)); } /** @@ -129,13 +230,14 @@ public class Matrix { } /** - *

- * Returns a matrix which is the result of an element by element multiplication of 'this' and 'b'. - * ci,j = ai,j*bi,j - *

+ * Returns a matrix which is the result of an element by element multiplication of + * "this" and other. * - * @param other A matrix. - * @return The element by element multiplication of 'this' and 'b'. + *

ci,j = ai,j*otheri,j + * + * + * @param other The other {@link Matrix} to preform element multiplication on. + * @return The element by element multiplication of "this" and other. */ public final Matrix elementTimes(Matrix other) { return new Matrix<>(this.m_storage.elementMult(Objects.requireNonNull(other).m_storage)); @@ -180,7 +282,7 @@ public class Matrix { * @return The resultant matrix. */ public final Matrix plus(Matrix value) { - return new Matrix<>(this.m_storage.plus(value.m_storage)); + return new Matrix<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage)); } /** @@ -206,7 +308,7 @@ public class Matrix { /** * Calculates the transpose, M^T of this matrix. * - * @return The tranpose matrix. + * @return The transpose matrix. */ public final Matrix transpose() { return new Matrix<>(this.m_storage.transpose()); @@ -224,15 +326,47 @@ public class Matrix { /** - * Returns the inverse matrix of this matrix. + * Returns the inverse matrix of "this" matrix. * - * @return The inverse of this matrix. - * @throws org.ejml.data.SingularMatrixException If this matrix is non-invertable. + * @return The inverse of "this" matrix. + * @throws org.ejml.data.SingularMatrixException If "this" matrix is non-invertable. */ public final Matrix inv() { return new Matrix<>(this.m_storage.invert()); } + /** + * Returns the solution x to the equation Ax = b, where A is "this" matrix. + * + *

The matrix equation could also be written as x = A-1b. Where the + * pseudo inverse is used if A is not square. + * + * @param b The right-hand side of the equation to solve. + * @return The solution to the linear system. + */ + @SuppressWarnings("ParameterName") + public final Matrix solve(Matrix b) { + return new Matrix<>(this.m_storage.solve(Objects.requireNonNull(b).m_storage)); + } + + /** + * Computes the matrix exponential using Eigen's solver. + * This method only works for square matrices, and will + * otherwise throw an {@link MatrixDimensionException}. + * + * @return The exponential of A. + */ + public final Matrix exp() { + if (this.getNumRows() != this.getNumCols()) { + throw new MatrixDimensionException("Non-square matrices cannot be exponentiated! " + + "This matrix is " + this.getNumRows() + " x " + this.getNumCols()); + } + Matrix toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols())); + WPIMathJNI.exp(this.m_storage.getDDRM().getData(), this.getNumRows(), + toReturn.m_storage.getDDRM().getData()); + return toReturn; + } + /** * Returns the determinant of this matrix. * @@ -243,9 +377,9 @@ public class Matrix { } /** - * Computes the Frobenius normal of the matrix.
- *
- * normF = Sqrt{ ∑i=1:m ∑j=1:n { aij2} } + * Computes the Frobenius normal of the matrix. + * + *

normF = Sqrt{ ∑i=1:m ∑j=1:n { aij2} } * * @return The matrix's Frobenius normal. */ @@ -254,9 +388,9 @@ public class Matrix { } /** - * Computes the induced p = 1 matrix norm.
- *
- * ||A||1= max(j=1 to n; sum(i=1 to m; |aij|)) + * Computes the induced p = 1 matrix norm. + * + *

||A||1= max(j=1 to n; sum(i=1 to m; |aij|)) * * @return The norm. */ @@ -283,45 +417,261 @@ public class Matrix { } /** - * Returns a matrix which is the result of an element by element power of 'this' and 'b': - * ci,j = ai,j ^ b. + * Returns a matrix which is the result of an element by element power of "this" and b. * - * @param b Scalar - * @return The element by element power of 'this' and 'b'. + *

ci,j = ai,j ^ b + * + * @param b Scalar. + * @return The element by element power of "this" and b. */ @SuppressWarnings("ParameterName") - public final Matrix epow(double b) { + public final Matrix elementPower(double b) { return new Matrix<>(this.m_storage.elementPower(b)); } /** - * Returns a matrix which is the result of an element by element power of 'this' and 'b': - * ci,j = ai,j ^ b. + * Returns a matrix which is the result of an element by element power of "this" and b. + * + *

ci,j = ai,j ^ b * * @param b Scalar. - * @return The element by element power of 'this' and 'b'. + * @return The element by element power of "this" and b. */ @SuppressWarnings("ParameterName") - public final Matrix epow(int b) { + public final Matrix elementPower(int b) { return new Matrix<>(this.m_storage.elementPower((double) b)); } /** - * Returns the EJML {@link SimpleMatrix} backing this wrapper. + * Extracts a given row into a row vector with new underlying storage. * - * @return The untyped EJML {@link SimpleMatrix}. + * @param row The row to extract a vector from. + * @return A row vector from the given row. */ - public final SimpleMatrix getStorage() { - return this.m_storage; + public final Matrix extractRowVector(int row) { + return new Matrix<>(this.m_storage.extractVector(true, row)); } /** - * Constructs a new matrix with the given storage. - * Caller should make sure that the provided generic bounds match the shape of the provided matrix + * Extracts a given column into a column vector with new underlying storage. * - * @param storage The {@link SimpleMatrix} to back this value + * @param column The column to extract a vector from. + * @return A column vector from the given column. */ - public Matrix(SimpleMatrix storage) { - this.m_storage = Objects.requireNonNull(storage); + public final Matrix extractColumnVector(int column) { + return new Matrix<>(this.m_storage.extractVector(false, column)); + } + + /** + * Extracts a matrix of a given size and start position with new underlying + * storage. + * + * @param height The number of rows of the extracted matrix. + * @param width The number of columns of the extracted matrix. + * @param startingRow The starting row of the extracted matrix. + * @param startingCol The starting column of the extracted matrix. + * @return The extracted matrix. + */ + public final Matrix block( + Nat height, Nat width, int startingRow, int startingCol) { + return new Matrix<>(this.m_storage.extractMatrix( + startingRow, + Objects.requireNonNull(height).getNum() + startingRow, + startingCol, + Objects.requireNonNull(width).getNum() + startingCol)); + } + + /** + * Assign a matrix of a given size and start position. + * + * @param startingRow The row to start at. + * @param startingCol The column to start at. + * @param other The matrix to assign the block to. + */ + public void assignBlock(int startingRow, int startingCol, + Matrix other) { + this.m_storage.insertIntoThis( + startingRow, + startingCol, + Objects.requireNonNull(other).m_storage); + } + + /** + * Extracts a submatrix from the supplied matrix and inserts it in a submatrix in "this". The + * shape of "this" is used to determine the size of the matrix extracted. + * + * @param startingRow The starting row in the supplied matrix to extract the submatrix. + * @param startingCol The starting column in the supplied matrix to extract the submatrix. + * @param other The matrix to extract the submatrix from. + */ + public void extractFrom(int startingRow, int startingCol, + Matrix other) { + CommonOps_DDRM.extract(other.m_storage.getDDRM(), startingRow, startingCol, + this.m_storage.getDDRM()); + } + + /** + * Decompose "this" matrix using Cholesky Decomposition. If the "this" matrix is zeros, it + * will return the zero matrix. + * + * @param lowerTriangular Whether or not we want to decompose to the lower triangular + * Cholesky matrix. + * @return The decomposed matrix. + * @throws RuntimeException if the matrix could not be decomposed(ie. is not positive + * semidefinite). + */ + @SuppressWarnings("PMD.AvoidThrowingRawExceptionTypes") + public Matrix lltDecompose(boolean lowerTriangular) { + SimpleMatrix temp = m_storage.copy(); + + CholeskyDecomposition_F64 chol = + DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular); + if (!chol.decompose(temp.getMatrix())) { + // check that the input is not all zeros -- if they are, we special case and return all + // zeros. + var matData = temp.getDDRM().data; + var isZeros = true; + for (double matDatum : matData) { + isZeros &= Math.abs(matDatum) < 1e-6; + } + if (isZeros) { + return new Matrix<>(new SimpleMatrix(temp.numRows(), temp.numCols())); + } + + throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + + m_storage.toString()); + } + + return new Matrix<>(SimpleMatrix.wrap(chol.getT(null))); + } + + /** + * Returns the row major data of this matrix as a double array. + * + * @return The row major data of this matrix as a double array. + */ + public double[] getData() { + return m_storage.getDDRM().getData(); + } + + /** + * Creates the identity matrix of the given dimension. + * + * @param dim The dimension of the desired matrix as a {@link Nat}. + * @param The dimension of the desired matrix as a generic. + * @return The DxD identity matrix. + */ + public static Matrix eye(Nat dim) { + return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum())); + } + + /** + * Creates the identity matrix of the given dimension. + * + * @param dim The dimension of the desired matrix as a {@link Num}. + * @param The dimension of the desired matrix as a generic. + * @return The DxD identity matrix. + */ + public static Matrix eye(D dim) { + return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum())); + } + + /** + * Entrypoint to the {@link MatBuilder} class for creation + * of custom matrices with the given dimensions and contents. + * + * @param rows The number of rows of the desired matrix. + * @param cols The number of columns of the desired matrix. + * @param The number of rows of the desired matrix as a generic. + * @param The number of columns of the desired matrix as a generic. + * @return A builder to construct the matrix. + */ + public static MatBuilder mat(Nat rows, Nat cols) { + return new MatBuilder<>(Objects.requireNonNull(rows), Objects.requireNonNull(cols)); + } + + /** + * Reassigns dimensions of a {@link Matrix} to allow for operations with + * other matrices that have wildcard dimensions. + * + * @param mat The {@link Matrix} to remove the dimensions from. + * @return The matrix with reassigned dimensions. + */ + public static Matrix changeBoundsUnchecked( + Matrix mat) { + return new Matrix<>(mat.m_storage); + } + + /** + * Checks if another {@link Matrix} is identical to "this" one within a specified tolerance. + * + *

This will check if each element is in tolerance of the corresponding element + * from the other {@link Matrix} or if the elements have the same symbolic meaning. For two + * elements to have the same symbolic meaning they both must be either Double.NaN, + * Double.POSITIVE_INFINITY, or Double.NEGATIVE_INFINITY. + * + *

NOTE:It is recommend to use {@link Matrix#isEqual(Matrix, double)} over this + * method when checking if two matrices are equal as {@link Matrix#isEqual(Matrix, double)} + * will return false if an element is uncountable. This method should only be used when + * uncountable elements need to compared. + * + * @param other The {@link Matrix} to check against this one. + * @param tolerance The tolerance to check equality with. + * @return true if this matrix is identical to the one supplied. + */ + public boolean isIdentical(Matrix other, double tolerance) { + return MatrixFeatures_DDRM.isIdentical(this.m_storage.getDDRM(), + other.m_storage.getDDRM(), tolerance); + } + + /** + * Checks if another {@link Matrix} is equal to "this" within a specified tolerance. + * + *

This will check if each element is in tolerance of the corresponding element + * from the other {@link Matrix}. + * + *

tol ≥ |aij - bij| + * + * @param other The {@link Matrix} to check against this one. + * @param tolerance The tolerance to check equality with. + * @return true if this matrix is equal to the one supplied. + */ + public boolean isEqual(Matrix other, double tolerance) { + return MatrixFeatures_DDRM.isEquals(this.m_storage.getDDRM(), + other.m_storage.getDDRM(), tolerance); + } + + @Override + public String toString() { + return m_storage.toString(); + } + + /** + * Checks if an object is equal to this {@link Matrix}. + * + *

aij == bij + * + * @param other The Object to check against this {@link Matrix}. + * @return true if the object supplied is a {@link Matrix} and is equal to this matrix. + */ + @Override + public boolean equals(Object other) { + if (this == other) { + return true; + } + if (!(other instanceof Matrix)) { + return false; + } + + Matrix matrix = (Matrix) other; + if (MatrixFeatures_DDRM.hasUncountable(matrix.m_storage.getDDRM())) { + return false; + } + return MatrixFeatures_DDRM.isEquals(this.m_storage.getDDRM(), matrix.m_storage.getDDRM()); + } + + @Override + public int hashCode() { + return Objects.hash(m_storage); } } diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java index 4ed38b073b..b3e47249a8 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java @@ -13,6 +13,7 @@ import org.ejml.simple.SimpleMatrix; import edu.wpi.first.wpiutil.math.numbers.N1; +@Deprecated public final class MatrixUtils { private MatrixUtils() { throw new AssertionError("utility class"); diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Pair.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Pair.java new file mode 100644 index 0000000000..eafbcba2f1 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Pair.java @@ -0,0 +1,31 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpiutil.math; + +public class Pair { + private final A m_first; + private final B m_second; + + public Pair(A first, B second) { + m_first = first; + m_second = second; + } + + public A getFirst() { + return m_first; + } + + public B getSecond() { + return m_second; + } + + @SuppressWarnings("ParameterName") + public static Pair of(A a, B b) { + return new Pair<>(a, b); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java index 6ace8b2669..ed53d12500 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java @@ -9,12 +9,17 @@ package edu.wpi.first.wpiutil.math; import java.util.function.BiFunction; +import org.ejml.data.DMatrixRMaj; import org.ejml.dense.row.NormOps_DDRM; +import org.ejml.dense.row.factory.DecompositionFactory_DDRM; +import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64; import org.ejml.simple.SimpleBase; import org.ejml.simple.SimpleMatrix; -public class SimpleMatrixUtils { - private SimpleMatrixUtils() {} +@SuppressWarnings("PMD.TooManyMethods") +public final class SimpleMatrixUtils { + private SimpleMatrixUtils() { + } /** * Compute the matrix exponential, e^M of the given matrix. @@ -98,8 +103,10 @@ public class SimpleMatrixUtils { SimpleMatrix A4 = A2.mult(A2); SimpleMatrix A6 = A4.mult(A2); - SimpleMatrix U = A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1]))); - SimpleMatrix V = A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])); + SimpleMatrix U = + A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1]))); + SimpleMatrix V = + A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])); return new Pair<>(U, V); } @@ -114,8 +121,10 @@ public class SimpleMatrixUtils { SimpleMatrix A6 = A4.mult(A2); SimpleMatrix A8 = A6.mult(A2); - SimpleMatrix U = A.mult(A8.scale(b[9]).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1]))); - SimpleMatrix V = A8.scale(b[8]).plus(A6.scale(b[6])).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])); + SimpleMatrix U = + A.mult(A8.scale(b[9]).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1]))); + SimpleMatrix V = + A8.scale(b[8]).plus(A6.scale(b[6])).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])); return new Pair<>(U, V); } @@ -131,8 +140,10 @@ public class SimpleMatrixUtils { SimpleMatrix A4 = A2.mult(A2); SimpleMatrix A6 = A4.mult(A2); - SimpleMatrix U = A.mult(A6.scale(b[13]).plus(A4.scale(b[11])).plus(A2.scale(b[9])).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1]))); - SimpleMatrix V = A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8]))).plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]))); + SimpleMatrix U = + A.mult(A6.scale(b[13]).plus(A4.scale(b[11])).plus(A2.scale(b[9])).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1]))); + SimpleMatrix V = + A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8]))).plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]))); return new Pair<>(U, V); } @@ -141,21 +152,76 @@ public class SimpleMatrixUtils { return SimpleMatrix.identity(Math.min(rows, cols)); } - private static class Pair { - private final A m_first; - private final B m_second; - - Pair(A first, B second) { - m_first = first; - m_second = second; - } - - public A getFirst() { - return m_first; - } - - public B getSecond() { - return m_second; - } + /** + * The identy of a square matrix. + * + * @param rows the number of rows (and columns) + * @return the identiy matrix, rows x rows. + */ + public static SimpleMatrix eye(int rows) { + return SimpleMatrix.identity(rows); } + + /** + * Decompose the given matrix using Cholesky Decomposition and return a view of the upper + * triangular matrix (if you want lower triangular see the other overload of this method.) If the + * input matrix is zeros, this will return the zero matrix. + * + * @param src The matrix to decompose. + * @return The decomposed matrix. + * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive + * semidefinite). + */ + public static SimpleMatrix lltDecompose(SimpleMatrix src) { + return lltDecompose(src, false); + } + + /** + * Decompose the given matrix using Cholesky Decomposition. If the input matrix is zeros, this + * will return the zero matrix. + * + * @param src The matrix to decompose. + * @param lowerTriangular if we want to decompose to the lower triangular Cholesky matrix. + * @return The decomposed matrix. + * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive + * semidefinite). + */ + @SuppressWarnings("PMD.AvoidThrowingRawExceptionTypes") + public static SimpleMatrix lltDecompose(SimpleMatrix src, boolean lowerTriangular) { + SimpleMatrix temp = src.copy(); + + CholeskyDecomposition_F64 chol = + DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular); + if (!chol.decompose(temp.getMatrix())) { + // check that the input is not all zeros -- if they are, we special case and return all + // zeros. + var matData = temp.getDDRM().data; + var isZeros = true; + for (double matDatum : matData) { + isZeros &= Math.abs(matDatum) < 1e-6; + } + if (isZeros) { + return new SimpleMatrix(temp.numRows(), temp.numCols()); + } + + throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + src.toString()); + } + + return SimpleMatrix.wrap(chol.getT(null)); + } + + /** + * Computes the matrix exponential using Eigen's solver. + * + * @param A the matrix to exponentiate. + * @return the exponential of A. + */ + @SuppressWarnings("ParameterName") + public static SimpleMatrix exp( + SimpleMatrix A) { + SimpleMatrix toReturn = new SimpleMatrix(A.numRows(), A.numRows()); + WPIMathJNI.exp(A.getDDRM().getData(), A.numRows(), toReturn.getDDRM().getData()); + return toReturn; + } + } diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java index fb20a90107..96075267e1 100644 --- a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java @@ -8,14 +8,171 @@ package edu.wpi.first.wpiutil.math; import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N10; +import edu.wpi.first.wpiutil.math.numbers.N2; +import edu.wpi.first.wpiutil.math.numbers.N3; +import edu.wpi.first.wpiutil.math.numbers.N4; +import edu.wpi.first.wpiutil.math.numbers.N5; +import edu.wpi.first.wpiutil.math.numbers.N6; +import edu.wpi.first.wpiutil.math.numbers.N7; +import edu.wpi.first.wpiutil.math.numbers.N8; +import edu.wpi.first.wpiutil.math.numbers.N9; + + /** * A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices). * * @param The dimension of the vector to be constructed. */ +@SuppressWarnings("PMD.TooManyMethods") public class VecBuilder extends MatBuilder { public VecBuilder(Nat rows) { super(rows, Nat.N1()); } + + private Vector fillVec(double... data) { + return new Vector<>(fill(data)); + } + + /** + * Returns a 1x1 vector containing the given elements. + * + * @param n1 the first element. + */ + public static Vector fill(double n1) { + return new VecBuilder<>(Nat.N1()).fillVec(n1); + } + + /** + * Returns a 2x1 vector containing the given elements. + * + * @param n1 the first element. + * @param n2 the second element. + */ + public static Vector fill(double n1, double n2) { + return new VecBuilder<>(Nat.N2()).fillVec(n1, n2); + } + + /** + * Returns a 3x1 vector containing the given elements. + * + * @param n1 the first element. + * @param n2 the second element. + * @param n3 the third element. + */ + public static Vector fill(double n1, double n2, double n3) { + return new VecBuilder<>(Nat.N3()).fillVec(n1, n2, n3); + } + + /** + * Returns a 4x1 vector containing the given elements. + * + * @param n1 the first element. + * @param n2 the second element. + * @param n3 the third element. + * @param n4 the fourth element. + */ + public static Vector fill(double n1, double n2, double n3, double n4) { + return new VecBuilder<>(Nat.N4()).fillVec(n1, n2, n3, n4); + } + + /** + * Returns a 5x1 vector containing the given elements. + * + * @param n1 the first element. + * @param n2 the second element. + * @param n3 the third element. + * @param n4 the fourth element. + * @param n5 the fifth element. + */ + public static Vector fill(double n1, double n2, double n3, double n4, double n5) { + return new VecBuilder<>(Nat.N5()).fillVec(n1, n2, n3, n4, n5); + } + + /** + * Returns a 6x1 vector containing the given elements. + * + * @param n1 the first element. + * @param n2 the second element. + * @param n3 the third element. + * @param n4 the fourth element. + * @param n5 the fifth element. + * @param n6 the sixth element. + */ + public static Vector fill(double n1, double n2, double n3, double n4, double n5, + double n6) { + return new VecBuilder<>(Nat.N6()).fillVec(n1, n2, n3, n4, n5, n6); + } + + /** + * Returns a 7x1 vector containing the given elements. + * + * @param n1 the first element. + * @param n2 the second element. + * @param n3 the third element. + * @param n4 the fourth element. + * @param n5 the fifth element. + * @param n6 the sixth element. + * @param n7 the seventh element. + */ + public static Vector fill(double n1, double n2, double n3, double n4, double n5, + double n6, double n7) { + return new VecBuilder<>(Nat.N7()).fillVec(n1, n2, n3, n4, n5, n6, n7); + } + + /** + * Returns a 8x1 vector containing the given elements. + * + * @param n1 the first element. + * @param n2 the second element. + * @param n3 the third element. + * @param n4 the fourth element. + * @param n5 the fifth element. + * @param n6 the sixth element. + * @param n7 the seventh element. + * @param n8 the eighth element. + */ + public static Vector fill(double n1, double n2, double n3, double n4, double n5, + double n6, double n7, double n8) { + return new VecBuilder<>(Nat.N8()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8); + } + + /** + * Returns a 9x1 vector containing the given elements. + * + * @param n1 the first element. + * @param n2 the second element. + * @param n3 the third element. + * @param n4 the fourth element. + * @param n5 the fifth element. + * @param n6 the sixth element. + * @param n7 the seventh element. + * @param n8 the eighth element. + * @param n9 the ninth element. + */ + public static Vector fill(double n1, double n2, double n3, double n4, double n5, + double n6, double n7, double n8, double n9) { + return new VecBuilder<>(Nat.N9()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9); + } + + /** + * Returns a 10x1 vector containing the given elements. + * + * @param n1 the first element. + * @param n2 the second element. + * @param n3 the third element. + * @param n4 the fourth element. + * @param n5 the fifth element. + * @param n6 the sixth element. + * @param n7 the seventh element. + * @param n8 the eighth element. + * @param n9 the ninth element. + * @param n10 the tenth element. + */ + @SuppressWarnings("PMD.ExcessiveParameterList") + public static Vector fill(double n1, double n2, double n3, double n4, double n5, + double n6, double n7, double n8, double n9, double n10) { + return new VecBuilder<>(Nat.N10()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10); + } } diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Vector.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Vector.java new file mode 100644 index 0000000000..7761367532 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Vector.java @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpiutil.math; + +import org.ejml.simple.SimpleMatrix; + +import edu.wpi.first.wpiutil.math.numbers.N1; + +/** + * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices. + * + *

This class is intended to be used alongside the state space library. + * + * @param The number of rows in this matrix. + */ +public class Vector extends Matrix { + + /** + * Constructs an empty zero vector of the given dimensions. + * + * @param rows The number of rows of the vector. + */ + public Vector(Nat rows) { + super(rows, Nat.N1()); + } + + /** + * Constructs a new {@link Vector} with the given storage. + * Caller should make sure that the provided generic bounds match + * the shape of the provided {@link Vector}. + * + *

NOTE:It is not recommended to use this constructor unless the + * {@link SimpleMatrix} API is absolutely necessary due to the desired + * function not being accessible through the {@link Vector} wrapper. + * + * @param storage The {@link SimpleMatrix} to back this vector. + */ + public Vector(SimpleMatrix storage) { + super(storage); + } + + /** + * Constructs a new vector with the storage of the supplied matrix. + * + * @param other The {@link Vector} to copy the storage of. + */ + public Vector(Matrix other) { + super(other); + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/wpiutil/math/WPIMathJNI.java b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/WPIMathJNI.java new file mode 100644 index 0000000000..0d47caa467 --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/wpiutil/math/WPIMathJNI.java @@ -0,0 +1,79 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpiutil.math; + +import java.io.IOException; +import java.util.concurrent.atomic.AtomicBoolean; + +import edu.wpi.first.wpiutil.RuntimeLoader; + +public final class WPIMathJNI { + static boolean libraryLoaded = false; + static RuntimeLoader loader = null; + + public static class Helper { + private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true); + + public static boolean getExtractOnStaticLoad() { + return extractOnStaticLoad.get(); + } + + public static void setExtractOnStaticLoad(boolean load) { + extractOnStaticLoad.set(load); + } + } + + static { + if (Helper.getExtractOnStaticLoad()) { + try { + loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class); + loader.loadLibrary(); + } catch (IOException ex) { + ex.printStackTrace(); + System.exit(1); + } + libraryLoaded = true; + } + } + + /** + * Force load the library. + */ + public static synchronized void forceLoad() throws IOException { + if (libraryLoaded) { + return; + } + loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class); + loader.loadLibrary(); + libraryLoaded = true; + } + + /** + * Computes the matrix exp. + * + * @param src Array of elements of the matrix to be exponentiated. + * @param rows how many rows there are. + * @param dst Array where the result will be stored. + */ + public static native void exp(double[] src, int rows, double[] dst); + + /** + * Returns true if (A, B) is a stabilizable pair. + * + *

(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if + * any, have absolute values less than one, where an eigenvalue is + * uncontrollable if rank(lambda * I - A, B) < n where n is number of states. + * + * @param states the number of states of the system. + * @param inputs the number of inputs to the system. + * @param A System matrix. + * @param B Input matrix. + * @return If the system is stabilizable. + */ + public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B); +} diff --git a/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp new file mode 100644 index 0000000000..d828f30730 --- /dev/null +++ b/wpimath/src/main/native/cpp/StateSpaceUtil.cpp @@ -0,0 +1,29 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/StateSpaceUtil.h" + +namespace frc { + +template <> +bool IsStabilizable<1, 1>(const Eigen::Matrix& A, + const Eigen::Matrix& B) { + return detail::IsStabilizableImpl<1, 1>(A, B); +} + +template <> +bool IsStabilizable<2, 1>(const Eigen::Matrix& A, + const Eigen::Matrix& B) { + return detail::IsStabilizableImpl<2, 1>(A, B); +} + +Eigen::Matrix PoseToVector(const Pose2d& pose) { + return frc::MakeMatrix<3, 1>(pose.X().to(), pose.Y().to(), + pose.Rotation().Radians().to()); +} + +} // namespace frc diff --git a/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp new file mode 100644 index 0000000000..543a2e95a9 --- /dev/null +++ b/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/controller/LinearQuadraticRegulator.h" + +namespace frc { + +LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const std::array& Qelems, const std::array& Relems, + units::second_t dt) + : LinearQuadraticRegulator(A, B, Qelems, 1.0, Relems, dt) {} + +LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const std::array& Qelems, const double rho, + const std::array& Relems, units::second_t dt) + : detail::LinearQuadraticRegulatorImpl<1, 1>{A, B, Qelems, + rho, Relems, dt} {} + +LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R, + units::second_t dt) + : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {} + +LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const std::array& Qelems, const std::array& Relems, + units::second_t dt) + : LinearQuadraticRegulator(A, B, Qelems, 1.0, Relems, dt) {} + +LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const std::array& Qelems, const double rho, + const std::array& Relems, units::second_t dt) + : detail::LinearQuadraticRegulatorImpl<2, 1>{A, B, Qelems, + rho, Relems, dt} {} + +LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator( + const Eigen::Matrix& A, const Eigen::Matrix& B, + const Eigen::Matrix& Q, const Eigen::Matrix& R, + units::second_t dt) + : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {} + +} // namespace frc diff --git a/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp b/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp new file mode 100644 index 0000000000..a1747ab014 --- /dev/null +++ b/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include "frc/estimator/KalmanFilter.h" + +namespace frc { + +KalmanFilter<1, 1, 1>::KalmanFilter( + LinearSystem<1, 1, 1>& plant, const std::array& stateStdDevs, + const std::array& measurementStdDevs, units::second_t dt) + : detail::KalmanFilterImpl<1, 1, 1>{plant, stateStdDevs, measurementStdDevs, + dt} {} + +KalmanFilter<2, 1, 1>::KalmanFilter( + LinearSystem<2, 1, 1>& plant, const std::array& stateStdDevs, + const std::array& measurementStdDevs, units::second_t dt) + : detail::KalmanFilterImpl<2, 1, 1>{plant, stateStdDevs, measurementStdDevs, + dt} {} + +} // namespace frc diff --git a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp index 33c10f8b2f..613c6cd3af 100644 --- a/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp +++ b/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp @@ -10,12 +10,39 @@ #include #include "Eigen/Core" +#include "Eigen/Eigenvalues" +#include "Eigen/QR" #include "drake/math/discrete_algebraic_riccati_equation.h" #include "edu_wpi_first_math_WPIMathJNI.h" #include "unsupported/Eigen/MatrixFunctions" using namespace wpi::java; +bool check_stabilizable(const Eigen::Ref& A, + const Eigen::Ref& B) { + // This function checks if (A,B) is a stabilizable pair. + // (A,B) is stabilizable if and only if the uncontrollable eigenvalues of + // A, if any, have absolute values less than one, where an eigenvalue is + // uncontrollable if Rank[lambda * I - A, B] < n. + int n = B.rows(), m = B.cols(); + Eigen::EigenSolver es(A); + for (int i = 0; i < n; i++) { + if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() + + es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() < + 1) + continue; + + Eigen::MatrixXcd E(n, n + m); + E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B; + Eigen::ColPivHouseholderQR qr(E); + if (qr.rank() != n) { + return false; + } + } + + return true; +} + extern "C" { /* @@ -57,4 +84,54 @@ Java_edu_wpi_first_math_WPIMathJNI_discreteAlgebraicRiccatiEquation env->SetDoubleArrayRegion(S, 0, states * states, result.data()); } +/* + * Class: edu_wpi_first_wpiutil_math_WPIMathJNI + * Method: exp + * Signature: ([DI[D)V + */ +JNIEXPORT void JNICALL +Java_edu_wpi_first_wpiutil_math_WPIMathJNI_exp + (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdoubleArray dst) +{ + jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr); + + Eigen::Map< + Eigen::Matrix> + Amat{arrayBody, rows, rows}; + Eigen::Matrix Aexp = + Amat.exp(); + + env->ReleaseDoubleArrayElements(src, arrayBody, 0); + env->SetDoubleArrayRegion(dst, 0, rows * rows, Aexp.data()); +} + +/* + * Class: edu_wpi_first_wpiutil_math_WPIMathJNI + * Method: isStabilizable + * Signature: (II[D[D)Z + */ +JNIEXPORT jboolean JNICALL +Java_edu_wpi_first_wpiutil_math_WPIMathJNI_isStabilizable + (JNIEnv* env, jclass, jint states, jint inputs, jdoubleArray aSrc, + jdoubleArray bSrc) +{ + jdouble* nativeA = env->GetDoubleArrayElements(aSrc, nullptr); + jdouble* nativeB = env->GetDoubleArrayElements(bSrc, nullptr); + + Eigen::Map< + Eigen::Matrix> + A{nativeA, states, states}; + + Eigen::Map< + Eigen::Matrix> + B{nativeB, states, inputs}; + + bool isStabilizable = check_stabilizable(A, B); + + env->ReleaseDoubleArrayElements(aSrc, nativeA, 0); + env->ReleaseDoubleArrayElements(bSrc, nativeB, 0); + + return isStabilizable; +} + } // extern "C" diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h new file mode 100644 index 0000000000..3de8da3182 --- /dev/null +++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h @@ -0,0 +1,303 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include +#include +#include + +#include "Eigen/Core" +#include "Eigen/Eigenvalues" +#include "Eigen/QR" +#include "frc/geometry/Pose2d.h" + +namespace frc { +namespace detail { + +template +void MatrixImpl(Matrix& result, T elem, Ts... elems) { + constexpr int count = Rows * Cols - (sizeof...(Ts) + 1); + + result(count / Cols, count % Cols) = elem; + if constexpr (sizeof...(Ts) > 0) { + MatrixImpl(result, elems...); + } +} + +template +void CostMatrixImpl(Matrix& result, T elem, Ts... elems) { + result(result.rows() - (sizeof...(Ts) + 1)) = 1.0 / std::pow(elem, 2); + if constexpr (sizeof...(Ts) > 0) { + CostMatrixImpl(result, elems...); + } +} + +template +void CovMatrixImpl(Matrix& result, T elem, Ts... elems) { + result(result.rows() - (sizeof...(Ts) + 1)) = std::pow(elem, 2); + if constexpr (sizeof...(Ts) > 0) { + CovMatrixImpl(result, elems...); + } +} + +template +void WhiteNoiseVectorImpl(Matrix& result, T elem, Ts... elems) { + std::random_device rd; + std::mt19937 gen{rd()}; + std::normal_distribution<> distr{0.0, elem}; + + result(result.rows() - (sizeof...(Ts) + 1)) = distr(gen); + if constexpr (sizeof...(Ts) > 0) { + WhiteNoiseVectorImpl(result, elems...); + } +} + +template +bool IsStabilizableImpl(const Eigen::Matrix& A, + const Eigen::Matrix& B) { + Eigen::EigenSolver> es(A); + + for (int i = 0; i < States; ++i) { + if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() + + es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() < + 1) { + continue; + } + + Eigen::Matrix, States, States + Inputs> E; + E << es.eigenvalues()[i] * Eigen::Matrix, States, + States>::Identity() - + A, + B; + + Eigen::ColPivHouseholderQR< + Eigen::Matrix, States, States + Inputs>> + qr(E); + if (qr.rank() < States) { + return false; + } + } + return true; +} + +} // namespace detail + +/** + * Creates a matrix from the given list of elements. + * + * The elements of the matrix are filled in in row-major order. + * + * @param elems An array of elements in the matrix. + * @return A matrix containing the given elements. + */ +template ...>>> +Eigen::Matrix MakeMatrix(Ts... elems) { + static_assert( + sizeof...(elems) == Rows * Cols, + "Number of provided elements doesn't match matrix dimensionality"); + + Eigen::Matrix result; + detail::MatrixImpl(result, elems...); + return result; +} + +/** + * Creates a cost matrix from the given vector for use with LQR. + * + * The cost matrix is constructed using Bryson's rule. The inverse square of + * each element in the input is taken and placed on the cost matrix diagonal. + * + * @param costs An array. For a Q matrix, its elements are the maximum allowed + * excursions of the states from the reference. For an R matrix, + * its elements are the maximum allowed excursions of the control + * inputs from no actuation. + * @return State excursion or control effort cost matrix. + */ +template ...>>> +Eigen::Matrix MakeCostMatrix( + Ts... costs) { + Eigen::DiagonalMatrix result; + detail::CostMatrixImpl(result.diagonal(), costs...); + return result; +} + +/** + * Creates a covariance matrix from the given vector for use with Kalman + * filters. + * + * Each element is squared and placed on the covariance matrix diagonal. + * + * @param stdDevs An array. For a Q matrix, its elements are the standard + * deviations of each state from how the model behaves. For an R + * matrix, its elements are the standard deviations for each + * output measurement. + * @return Process noise or measurement noise covariance matrix. + */ +template ...>>> +Eigen::Matrix MakeCovMatrix( + Ts... stdDevs) { + Eigen::DiagonalMatrix result; + detail::CovMatrixImpl(result.diagonal(), stdDevs...); + return result; +} + +/** + * Creates a cost matrix from the given vector for use with LQR. + * + * The cost matrix is constructed using Bryson's rule. The inverse square of + * each element in the input is taken and placed on the cost matrix diagonal. + * + * @param costs An array. For a Q matrix, its elements are the maximum allowed + * excursions of the states from the reference. For an R matrix, + * its elements are the maximum allowed excursions of the control + * inputs from no actuation. + * @return State excursion or control effort cost matrix. + */ +template +Eigen::Matrix MakeCostMatrix(const std::array& costs) { + Eigen::DiagonalMatrix result; + auto& diag = result.diagonal(); + for (size_t i = 0; i < N; ++i) { + diag(i) = 1.0 / std::pow(costs[i], 2); + } + return result; +} + +/** + * Creates a covariance matrix from the given vector for use with Kalman + * filters. + * + * Each element is squared and placed on the covariance matrix diagonal. + * + * @param stdDevs An array. For a Q matrix, its elements are the standard + * deviations of each state from how the model behaves. For an R + * matrix, its elements are the standard deviations for each + * output measurement. + * @return Process noise or measurement noise covariance matrix. + */ +template +Eigen::Matrix MakeCovMatrix( + const std::array& stdDevs) { + Eigen::DiagonalMatrix result; + auto& diag = result.diagonal(); + for (size_t i = 0; i < N; ++i) { + diag(i) = std::pow(stdDevs[i], 2); + } + return result; +} + +template ...>>> +Eigen::Matrix MakeWhiteNoiseVector(Ts... stdDevs) { + Eigen::Matrix result; + detail::WhiteNoiseVectorImpl(result, stdDevs...); + return result; +} + +/** + * Creates a vector of normally distributed white noise with the given noise + * intensities for each element. + * + * @param stdDevs An array whose elements are the standard deviations of each + * element of the noise vector. + * @return White noise vector. + */ +template +Eigen::Matrix MakeWhiteNoiseVector( + const std::array& stdDevs) { + std::random_device rd; + std::mt19937 gen{rd()}; + + Eigen::Matrix result; + for (int i = 0; i < N; ++i) { + std::normal_distribution<> distr{0.0, stdDevs[i]}; + result(i) = distr(gen); + } + return result; +} + +/** + * Returns true if (A, B) is a stabilizable pair. + * + * (A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if + * any, have absolute values less than one, where an eigenvalue is + * uncontrollable if rank(lambda * I - A, B) < n where n is number of states. + * + * @param A System matrix. + * @param B Input matrix. + */ +template +bool IsStabilizable(const Eigen::Matrix& A, + const Eigen::Matrix& B) { + return detail::IsStabilizableImpl(A, B); +} + +// Template specializations are used here to make common state-input pairs +// compile faster. +template <> +bool IsStabilizable<1, 1>(const Eigen::Matrix& A, + const Eigen::Matrix& B); + +// Template specializations are used here to make common state-input pairs +// compile faster. +template <> +bool IsStabilizable<2, 1>(const Eigen::Matrix& A, + const Eigen::Matrix& B); + +/** + * Converts a Pose2d into a vector of [x, y, theta]. + * + * @param pose The pose that is being represented. + * + * @return The vector. + */ +Eigen::Matrix PoseToVector(const Pose2d& pose); + +/** + * Clamps input vector between system's minimum and maximum allowable input. + * + * @param u Input vector to clamp. + * @return Clamped input vector. + */ +template +Eigen::Matrix ClampInputMaxMagnitude( + const Eigen::Matrix& u, + const Eigen::Matrix& umin, + const Eigen::Matrix& umax) { + Eigen::Matrix result; + for (int i = 0; i < Inputs; ++i) { + result(i) = std::clamp(u(i), umin(i), umax(i)); + } + return result; +} + +/** + * Normalize all inputs if any excedes the maximum magnitude. Useful for systems + * such as differential drivetrains. + * + * @param u The input vector. + * @param maxMagnitude The maximum magnitude any input can have. + * @param The number of inputs. + * @return The normalizedInput + */ +template +Eigen::Matrix NormalizeInputVector( + const Eigen::Matrix& u, double maxMagnitude) { + double maxValue = u.template lpNorm(); + + if (maxValue > maxMagnitude) { + return u * maxMagnitude / maxValue; + } + return u; +} +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h new file mode 100644 index 0000000000..7d88e77ddd --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h @@ -0,0 +1,202 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "Eigen/Core" +#include "frc/system/NumericalJacobian.h" +#include "units/time.h" + +namespace frc { + +/** + * Constructs a control-affine plant inversion model-based feedforward from + * given model dynamics. + * + * If given the vector valued function as f(x, u) where x is the state + * vector and u is the input vector, the B matrix(continuous input matrix) + * is calculated through a NumericalJacobian. In this case f has to be + * control-affine (of the form f(x) + Bu). + * + * The feedforward is calculated as + * u_ff = B+ (rDot - f(x)) , where + * B+ is the pseudoinverse of B. + * + * This feedforward does not account for a dynamic B matrix, B is either + * determined or supplied when the feedforward is created and remains constant. + * + * For more on the underlying math, read + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf. + */ +template +class ControlAffinePlantInversionFeedforward { + public: + /** + * Constructs a feedforward with given model dynamics as a function + * of state and input. + * + * @param f A vector-valued function of x, the state, and + * u, the input, that returns the derivative of + * the state vector. HAS to be control-affine + * (of the form f(x) + Bu). + * @param dt The timestep between calls of calculate(). + */ + ControlAffinePlantInversionFeedforward( + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + f, + units::second_t dt) + : m_dt(dt), m_f(f) { + m_B = NumericalJacobianU( + f, Eigen::Matrix::Zero(), + Eigen::Matrix::Zero()); + + m_r.setZero(); + Reset(m_r); + } + + /** + * Constructs a feedforward with given model dynamics as a function of state, + * and the plant's B matrix(continuous input matrix). + * + * @param f A vector-valued function of x, the state, + * that returns the derivative of the state vector. + * @param B Continuous input matrix of the plant being controlled. + * @param dt The timestep between calls of calculate(). + */ + ControlAffinePlantInversionFeedforward( + std::function( + const Eigen::Matrix&)> + f, + const Eigen::Matrix& B, units::second_t dt) + : m_B(B), m_dt(dt) { + m_f = [=](const Eigen::Matrix& x, + const Eigen::Matrix& u) + -> Eigen::Matrix { return f(x); }; + + m_r.setZero(); + Reset(m_r); + } + + ControlAffinePlantInversionFeedforward( + ControlAffinePlantInversionFeedforward&&) = default; + ControlAffinePlantInversionFeedforward& operator=( + ControlAffinePlantInversionFeedforward&&) = default; + + /** + * Returns the previously calculated feedforward as an input vector. + * + * @return The calculated feedforward. + */ + const Eigen::Matrix& Uff() const { return m_uff; } + + /** + * Returns an element of the previously calculated feedforward. + * + * @param row Row of uff. + * + * @return The row of the calculated feedforward. + */ + double Uff(int i) const { return m_uff(i, 0); } + + /** + * Returns the current reference vector r. + * + * @return The current reference vector. + */ + const Eigen::Matrix& R() const { return m_r; } + + /** + * Returns an element of the reference vector r. + * + * @param i Row of r. + * + * @return The row of the current reference vector. + */ + double R(int i) const { return m_r(i, 0); } + + /** + * Resets the feedforward with a specified initial state vector. + * + * @param initialState The initial state vector. + */ + void Reset(const Eigen::Matrix& initialState) { + m_r = initialState; + m_uff.setZero(); + } + + /** + * Resets the feedforward with a zero initial state vector. + */ + void Reset() { + m_r.setZero(); + m_uff.setZero(); + } + + /** + * Calculate the feedforward with only the desired + * future reference. This uses the internally stored "current" + * reference. + * + * If this method is used the initial state of the system is the one + * set using Reset(const Eigen::Matrix&). + * If the initial state is not set it defaults to a zero vector. + * + * @param nextR The reference state of the future timestep (k + dt). + * + * @return The calculated feedforward. + */ + Eigen::Matrix Calculate( + const Eigen::Matrix& nextR) { + return Calculate(m_r, nextR); + } + + /** + * Calculate the feedforward with current and future reference vectors. + * + * @param r The reference state of the current timestep (k). + * @param nextR The reference state of the future timestep (k + dt). + * + * @return The calculated feedforward. + */ + Eigen::Matrix Calculate( + const Eigen::Matrix& r, + const Eigen::Matrix& nextR) { + Eigen::Matrix rDot = (nextR - r) / m_dt.to(); + + m_uff = m_B.householderQr().solve( + rDot - m_f(r, Eigen::Matrix::Zero())); + + m_r = nextR; + return m_uff; + } + + private: + Eigen::Matrix m_B; + + units::second_t m_dt; + + /** + * The model dynamics. + */ + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + m_f; + + // Current reference + Eigen::Matrix m_r; + + // Computed feedforward + Eigen::Matrix m_uff; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h new file mode 100644 index 0000000000..9f0d7f2061 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h @@ -0,0 +1,162 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "Eigen/Core" +#include "frc/system/Discretization.h" +#include "frc/system/LinearSystem.h" +#include "units/time.h" + +namespace frc { + +/** + * Constructs a plant inversion model-based feedforward from a LinearSystem. + * + * The feedforward is calculated as u_ff = B+ (r_k+1 - A + * r_k) , where B+ is the pseudoinverse + * of B. + * + * For more on the underlying math, read + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf. + */ +template +class LinearPlantInversionFeedforward { + public: + /** + * Constructs a feedforward with the given plant. + * + * @param plant The plant being controlled. + * @param dtSeconds Discretization timestep. + */ + template + LinearPlantInversionFeedforward( + const LinearSystem& plant, units::second_t dt) + : LinearPlantInversionFeedforward(plant.A(), plant.B(), dt) {} + + /** + * Constructs a feedforward with the given coefficients. + * + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param dtSeconds Discretization timestep. + */ + LinearPlantInversionFeedforward( + const Eigen::Matrix& A, + const Eigen::Matrix& B, units::second_t dt) + : m_dt(dt) { + DiscretizeAB(A, B, dt, &m_A, &m_B); + + m_r.setZero(); + Reset(m_r); + } + + LinearPlantInversionFeedforward(LinearPlantInversionFeedforward&&) = default; + LinearPlantInversionFeedforward& operator=( + LinearPlantInversionFeedforward&&) = default; + + /** + * Returns the previously calculated feedforward as an input vector. + * + * @return The calculated feedforward. + */ + const Eigen::Matrix& Uff() const { return m_uff; } + + /** + * Returns an element of the previously calculated feedforward. + * + * @param row Row of uff. + * + * @return The row of the calculated feedforward. + */ + double Uff(int i) const { return m_uff(i, 0); } + + /** + * Returns the current reference vector r. + * + * @return The current reference vector. + */ + const Eigen::Matrix& R() const { return m_r; } + + /** + * Returns an element of the reference vector r. + * + * @param i Row of r. + * + * @return The row of the current reference vector. + */ + double R(int i) const { return m_r(i, 0); } + + /** + * Resets the feedforward with a specified initial state vector. + * + * @param initialState The initial state vector. + */ + void Reset(const Eigen::Matrix& initialState) { + m_r = initialState; + m_uff.setZero(); + } + + /** + * Resets the feedforward with a zero initial state vector. + */ + void Reset() { + m_r.setZero(); + m_uff.setZero(); + } + + /** + * Calculate the feedforward with only the desired + * future reference. This uses the internally stored "current" + * reference. + * + * If this method is used the initial state of the system is the one + * set using Reset(const Eigen::Matrix&). + * If the initial state is not set it defaults to a zero vector. + * + * @param nextR The reference state of the future timestep (k + dt). + * + * @return The calculated feedforward. + */ + Eigen::Matrix Calculate( + const Eigen::Matrix& nextR) { + return Calculate(m_r, nextR); + } + + /** + * Calculate the feedforward with current and future reference vectors. + * + * @param r The reference state of the current timestep (k). + * @param nextR The reference state of the future timestep (k + dt). + * + * @return The calculated feedforward. + */ + Eigen::Matrix Calculate( + const Eigen::Matrix& r, + const Eigen::Matrix& nextR) { + m_uff = m_B.householderQr().solve(nextR - (m_A * r)); + m_r = nextR; + return m_uff; + } + + private: + Eigen::Matrix m_A; + Eigen::Matrix m_B; + + units::second_t m_dt; + + // Current reference + Eigen::Matrix m_r; + + // Computed feedforward + Eigen::Matrix m_uff; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h new file mode 100644 index 0000000000..b6b8d9a87f --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h @@ -0,0 +1,422 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "Eigen/Core" +#include "Eigen/QR" +#include "drake/math/discrete_algebraic_riccati_equation.h" +#include "frc/StateSpaceUtil.h" +#include "frc/system/Discretization.h" +#include "frc/system/LinearSystem.h" +#include "units/time.h" + +namespace frc { +namespace detail { + +/** + * Contains the controller coefficients and logic for a linear-quadratic + * regulator (LQR). + * LQRs use the control law u = K(r - x). + * + * For more on the underlying math, read + * https://file.tavsys.net/control/controls-engineering-in-frc.pdf. + */ +template +class LinearQuadraticRegulatorImpl { + public: + /** + * Constructs a controller with the given coefficients and plant. + * + * @param plant The plant being controlled. + * @param Qelems The maximum desired error tolerance for each state. + * @param Relems The maximum desired control effort for each input. + * @param dt Discretization timestep. + */ + template + LinearQuadraticRegulatorImpl( + const LinearSystem& plant, + const std::array& Qelems, + const std::array& Relems, units::second_t dt) + : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, 1.0, Relems, + dt) {} + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param plant The plant being controlled. + * @param Qelems The maximum desired error tolerance for each state. + * @param rho A weighting factor that balances control effort and state + * excursion. Greater values penalize state excursion more heavily. 1 is a + * good starting value. + * @param Relems The maximum desired control effort for each input. + * @param dt Discretization timestep. + */ + template + LinearQuadraticRegulatorImpl( + const LinearSystem& plant, + const std::array& Qelems, const double rho, + const std::array& Relems, units::second_t dt) + : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, rho, Relems, + dt) {} + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param Qelems The maximum desired error tolerance for each state. + * @param rho A weighting factor that balances control effort and state + * excursion. Greater values penalize state excursion more heavily. 1 is a + * good starting value. + * @param Relems The maximum desired control effort for each input. + * @param dt Discretization timestep. + */ + LinearQuadraticRegulatorImpl(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const std::array& Qelems, + const std::array& Relems, + units::second_t dt) + : LinearQuadraticRegulatorImpl(A, B, Qelems, 1.0, Relems, dt) {} + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param Qelems The maximum desired error tolerance for each state. + * @param rho A weighting factor that balances control effort and state + * excursion. Greater values penalize state excursion more heavily. 1 is a + * good starting value. + * @param Relems The maximum desired control effort for each input. + * @param dt Discretization timestep. + */ + LinearQuadraticRegulatorImpl(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const std::array& Qelems, + const double rho, + const std::array& Relems, + units::second_t dt) + : LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems) * rho, + MakeCostMatrix(Relems), dt) {} + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param Q The state cost matrix. + * @param R The input cost matrix. + * @param dt Discretization timestep. + */ + LinearQuadraticRegulatorImpl(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, + units::second_t dt) { + Eigen::Matrix discA; + Eigen::Matrix discB; + DiscretizeAB(A, B, dt, &discA, &discB); + + Eigen::Matrix S = + drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R); + Eigen::Matrix tmp = + discB.transpose() * S * discB + R; + m_K = tmp.llt().solve(discB.transpose() * S * discA); + + Reset(); + } + + LinearQuadraticRegulatorImpl(LinearQuadraticRegulatorImpl&&) = default; + LinearQuadraticRegulatorImpl& operator=(LinearQuadraticRegulatorImpl&&) = + default; + + /** + * Returns the controller matrix K. + */ + const Eigen::Matrix& K() const { return m_K; } + + /** + * Returns an element of the controller matrix K. + * + * @param i Row of K. + * @param j Column of K. + */ + double K(int i, int j) const { return m_K(i, j); } + + /** + * Returns the reference vector r. + * + * @return The reference vector. + */ + const Eigen::Matrix& R() const { return m_r; } + + /** + * Returns an element of the reference vector r. + * + * @param i Row of r. + * + * @return The row of the reference vector. + */ + double R(int i) const { return m_r(i, 0); } + + /** + * Returns the control input vector u. + * + * @return The control input. + */ + const Eigen::Matrix& U() const { return m_u; } + + /** + * Returns an element of the control input vector u. + * + * @param i Row of u. + * + * @return The row of the control input vector. + */ + double U(int i) const { return m_u(i, 0); } + + /** + * Resets the controller. + */ + void Reset() { + m_r.setZero(); + m_u.setZero(); + } + + /** + * Returns the next output of the controller. + * + * @param x The current state x. + */ + Eigen::Matrix Calculate( + const Eigen::Matrix& x) { + m_u = m_K * (m_r - x); + return m_u; + } + + /** + * Returns the next output of the controller. + * + * @param x The current state x. + * @param nextR The next reference vector r. + */ + Eigen::Matrix Calculate( + const Eigen::Matrix& x, + const Eigen::Matrix& nextR) { + m_r = nextR; + return Calculate(x); + } + + private: + // Current reference + Eigen::Matrix m_r; + + // Computed controller output + Eigen::Matrix m_u; + + // Controller gain + Eigen::Matrix m_K; +}; + +} // namespace detail + +template +class LinearQuadraticRegulator + : public detail::LinearQuadraticRegulatorImpl { + public: + /** + * Constructs a controller with the given coefficients and plant. + * + * @param system The plant being controlled. + * @param Qelems The maximum desired error tolerance for each state. + * @param Relems The maximum desired control effort for each input. + * @param dt Discretization timestep. + */ + template + LinearQuadraticRegulator(const LinearSystem& plant, + const std::array& Qelems, + const std::array& Relems, + units::second_t dt) + : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, 1.0, Relems, + dt) {} + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param system The plant being controlled. + * @param Qelems The maximum desired error tolerance for each state. + * @param rho A weighting factor that balances control effort and state + * excursion. Greater values penalize state excursion more heavily. 1 is a + * good starting value. + * @param Relems The maximum desired control effort for each input. + * @param dt Discretization timestep. + */ + template + LinearQuadraticRegulator(const LinearSystem& plant, + const std::array& Qelems, + const double rho, + const std::array& Relems, + units::second_t dt) + : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, rho, Relems, + dt) {} + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param Qelems The maximum desired error tolerance for each state. + * @param rho A weighting factor that balances control effort and state + * excursion. Greater values penalize state excursion more heavily. 1 is a + * good starting value. + * @param Relems The maximum desired control effort for each input. + * @param dt Discretization timestep. + */ + LinearQuadraticRegulator(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const std::array& Qelems, + const std::array& Relems, + units::second_t dt) + : LinearQuadraticRegulator(A, B, Qelems, 1.0, Relems, dt) {} + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param Qelems The maximum desired error tolerance for each state. + * @param rho A weighting factor that balances control effort and state + * excursion. Greater values penalize state excursion more heavily. 1 is a + * good starting value. + * @param Relems The maximum desired control effort for each input. + * @param dt Discretization timestep. + */ + LinearQuadraticRegulator(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const std::array& Qelems, + const double rho, + const std::array& Relems, + units::second_t dt) + : detail::LinearQuadraticRegulatorImpl{ + A, B, Qelems, rho, Relems, dt} {} + + /** + * Constructs a controller with the given coefficients and plant. + * + * @param A Continuous system matrix of the plant being controlled. + * @param B Continuous input matrix of the plant being controlled. + * @param Q The state cost matrix. + * @param R The input cost matrix. + * @param dt Discretization timestep. + */ + LinearQuadraticRegulator(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, + units::second_t dt) + : detail::LinearQuadraticRegulatorImpl{A, B, Q, R, dt} {} + + LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default; + LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default; +}; + +// Template specializations are used here to make common state-input pairs +// compile faster. +template <> +class LinearQuadraticRegulator<1, 1> + : public detail::LinearQuadraticRegulatorImpl<1, 1> { + public: + template + LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant, + const std::array& Qelems, + const std::array& Relems, + units::second_t dt) + : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, 1.0, Relems, + dt) {} + + template + LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant, + const std::array& Qelems, + const double rho, + const std::array& Relems, + units::second_t dt) + : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, rho, Relems, + dt) {} + + LinearQuadraticRegulator(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const std::array& Qelems, + const std::array& Relems, + units::second_t dt); + + LinearQuadraticRegulator(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const std::array& Qelems, + const double rho, + const std::array& Relems, + units::second_t dt); + + LinearQuadraticRegulator(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, + units::second_t dt); + + LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default; + LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default; +}; + +// Template specializations are used here to make common state-input pairs +// compile faster. +template <> +class LinearQuadraticRegulator<2, 1> + : public detail::LinearQuadraticRegulatorImpl<2, 1> { + public: + template + LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant, + const std::array& Qelems, + const std::array& Relems, + units::second_t dt) + : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, 1.0, Relems, + dt) {} + + template + LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant, + const std::array& Qelems, + const double rho, + const std::array& Relems, + units::second_t dt) + : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, rho, Relems, + dt) {} + + LinearQuadraticRegulator(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const std::array& Qelems, + const std::array& Relems, + units::second_t dt); + + LinearQuadraticRegulator(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const std::array& Qelems, + const double rho, + const std::array& Relems, + units::second_t dt); + + LinearQuadraticRegulator(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, + units::second_t dt); + + LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default; + LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h new file mode 100644 index 0000000000..a9137760a5 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h @@ -0,0 +1,230 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "Eigen/Cholesky" +#include "Eigen/Core" +#include "drake/math/discrete_algebraic_riccati_equation.h" +#include "frc/StateSpaceUtil.h" +#include "frc/system/Discretization.h" +#include "frc/system/NumericalJacobian.h" +#include "frc/system/RungeKutta.h" +#include "units/time.h" + +namespace frc { + +template +class ExtendedKalmanFilter { + public: + /** + * Constructs an extended Kalman filter. + * + * @param f A vector-valued function of x and u that returns + * the derivative of the state vector. + * @param h A vector-valued function of x and u that returns + * the measurement vector. + * @param stateStdDevs Standard deviations of model states. + * @param measurementStdDevs Standard deviations of measurements. + * @param dt Nominal discretization timestep. + */ + ExtendedKalmanFilter(std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + f, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + h, + const std::array& stateStdDevs, + const std::array& measurementStdDevs, + units::second_t dt) + : m_f(f), m_h(h) { + m_contQ = MakeCovMatrix(stateStdDevs); + m_contR = MakeCovMatrix(measurementStdDevs); + + Reset(); + + Eigen::Matrix contA = + NumericalJacobianX( + m_f, m_xHat, Eigen::Matrix::Zero()); + Eigen::Matrix C = + NumericalJacobianX( + m_h, m_xHat, Eigen::Matrix::Zero()); + + Eigen::Matrix discA; + Eigen::Matrix discQ; + DiscretizeAQTaylor(contA, m_contQ, dt, &discA, &discQ); + + m_discR = DiscretizeR(m_contR, dt); + + // IsStabilizable(A^T, C^T) will tell us if the system is observable. + bool isObservable = + IsStabilizable(discA.transpose(), C.transpose()); + if (isObservable && Outputs <= States) { + m_initP = drake::math::DiscreteAlgebraicRiccatiEquation( + discA.transpose(), C.transpose(), discQ, m_discR); + } else { + m_initP = Eigen::Matrix::Zero(); + } + m_P = m_initP; + } + + /** + * Returns the error covariance matrix P. + */ + const Eigen::Matrix& P() const { return m_P; } + + /** + * Returns an element of the error covariance matrix P. + * + * @param i Row of P. + * @param j Column of P. + */ + double P(int i, int j) const { return m_P(i, j); } + + /** + * Set the current error covariance matrix P. + * + * @param P The error covariance matrix P. + */ + void SetP(const Eigen::Matrix& P) { m_P = P; } + + /** + * Returns the state estimate x-hat. + */ + const Eigen::Matrix& Xhat() const { return m_xHat; } + + /** + * Returns an element of the state estimate x-hat. + * + * @param i Row of x-hat. + */ + double Xhat(int i) const { return m_xHat(i, 0); } + + /** + * Set initial state estimate x-hat. + * + * @param xHat The state estimate x-hat. + */ + void SetXhat(const Eigen::Matrix& xHat) { m_xHat = xHat; } + + /** + * Set an element of the initial state estimate x-hat. + * + * @param i Row of x-hat. + * @param value Value for element of x-hat. + */ + void SetXhat(int i, double value) { m_xHat(i, 0) = value; } + + /** + * Resets the observer. + */ + void Reset() { + m_xHat.setZero(); + m_P = m_initP; + } + + /** + * Project the model into the future with a new control input u. + * + * @param u New control input from controller. + * @param dt Timestep for prediction. + */ + void Predict(const Eigen::Matrix& u, units::second_t dt) { + // Find continuous A + Eigen::Matrix contA = + NumericalJacobianX(m_f, m_xHat, u); + + // Find discrete A and Q + Eigen::Matrix discA; + Eigen::Matrix discQ; + DiscretizeAQTaylor(contA, m_contQ, dt, &discA, &discQ); + + m_xHat = RungeKutta(m_f, m_xHat, u, dt); + m_P = discA * m_P * discA.transpose() + discQ; + m_discR = DiscretizeR(m_contR, dt); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * @param u Same control input used in the predict step. + * @param y Measurement vector. + */ + void Correct(const Eigen::Matrix& u, + const Eigen::Matrix& y) { + Correct(u, y, m_h, m_discR); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * This is useful for when the measurements available during a timestep's + * Correct() call vary. The h(x, u) passed to the constructor is used if one + * is not provided (the two-argument version of this function). + * + * @param u Same control input used in the predict step. + * @param y Measurement vector. + * @param h A vector-valued function of x and u that returns + * the measurement vector. + * @param R Discrete measurement noise covariance matrix. + */ + template + void Correct(const Eigen::Matrix& u, + const Eigen::Matrix& y, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + h, + const Eigen::Matrix& R) { + const Eigen::Matrix C = + NumericalJacobianX(h, m_xHat, u); + + Eigen::Matrix S = C * m_P * C.transpose() + R; + + // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more + // efficiently. + // + // K = PC^T S^-1 + // KS = PC^T + // (KS)^T = (PC^T)^T + // S^T K^T = CP^T + // + // The solution of Ax = b can be found via x = A.solve(b). + // + // K^T = S^T.solve(CP^T) + // K = (S^T.solve(CP^T))^T + Eigen::Matrix K = + S.transpose().ldlt().solve(C * m_P.transpose()).transpose(); + + m_xHat += K * (y - h(m_xHat, u)); + m_P = (Eigen::Matrix::Identity() - K * C) * m_P; + } + + private: + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + m_f; + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + m_h; + Eigen::Matrix m_xHat; + Eigen::Matrix m_P; + Eigen::Matrix m_contQ; + Eigen::Matrix m_contR; + Eigen::Matrix m_discR; + + Eigen::Matrix m_initP; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h new file mode 100644 index 0000000000..4bff9d5ac5 --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h @@ -0,0 +1,291 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "Eigen/Core" +#include "drake/math/discrete_algebraic_riccati_equation.h" +#include "frc/StateSpaceUtil.h" +#include "frc/system/Discretization.h" +#include "frc/system/LinearSystem.h" +#include "units/time.h" +#include "wpimath/MathShared.h" + +namespace frc { +namespace detail { + +/** + * Luenberger observers combine predictions from a model and measurements to + * give an estimate of the true system state. + * + * Luenberger observers use an L gain matrix to determine whether to trust the + * model or measurements more. Kalman filter theory uses statistics to compute + * an optimal L gain (alternatively called the Kalman gain, K) which minimizes + * the sum of squares error in the state estimate. + * + * Luenberger observers run the prediction and correction steps simultaneously + * while Kalman filters run them sequentially. To implement a discrete-time + * Kalman filter as a Luenberger observer, use the following mapping: + * 

C = H, L = A * K
+ * (H is the measurement matrix). + * + * For more on the underlying math, read + * https://file.tavsys.net/control/state-space-guide.pdf. + */ +template +class KalmanFilterImpl { + public: + /** + * Constructs a state-space observer with the given plant. + * + * @param plant The plant used for the prediction step. + * @param stateStdDevs Standard deviations of model states. + * @param measurementStdDevs Standard deviations of measurements. + * @param dt Nominal discretization timestep. + */ + KalmanFilterImpl(LinearSystem& plant, + const std::array& stateStdDevs, + const std::array& measurementStdDevs, + units::second_t dt) { + m_plant = &plant; + + m_contQ = MakeCovMatrix(stateStdDevs); + m_contR = MakeCovMatrix(measurementStdDevs); + + Eigen::Matrix discA; + Eigen::Matrix discQ; + DiscretizeAQTaylor(plant.A(), m_contQ, dt, &discA, &discQ); + + m_discR = DiscretizeR(m_contR, dt); + + // IsStabilizable(A^T, C^T) will tell us if the system is observable. + bool isObservable = IsStabilizable(discA.transpose(), + plant.C().transpose()); + if (isObservable) { + if (Outputs <= States) { + m_P = drake::math::DiscreteAlgebraicRiccatiEquation( + discA.transpose(), plant.C().transpose(), discQ, m_discR); + } else { + m_P.setZero(); + } + } else { + wpi::math::MathSharedStore::ReportError( + "The system passed to the Kalman Filter is not observable!"); + throw std::invalid_argument( + "The system passed to the Kalman Filter is not observable!"); + } + } + + KalmanFilterImpl(KalmanFilterImpl&&) = default; + KalmanFilterImpl& operator=(KalmanFilterImpl&&) = default; + + /** + * Returns the error covariance matrix P. + */ + const Eigen::Matrix& P() const { return m_P; } + + /** + * Returns an element of the error covariance matrix P. + * + * @param i Row of P. + * @param j Column of P. + */ + double P(int i, int j) const { return m_P(i, j); } + + /** + * Set the current error covariance matrix P. + * + * @param P The error covariance matrix P. + */ + void SetP(const Eigen::Matrix& P) { m_P = P; } + + /** + * Returns the state estimate x-hat. + */ + const Eigen::Matrix& Xhat() const { return m_xHat; } + + /** + * Returns an element of the state estimate x-hat. + * + * @param i Row of x-hat. + */ + double Xhat(int i) const { return m_xHat(i); } + + /** + * Set initial state estimate x-hat. + * + * @param xHat The state estimate x-hat. + */ + void SetXhat(const Eigen::Matrix& xHat) { m_xHat = xHat; } + + /** + * Set an element of the initial state estimate x-hat. + * + * @param i Row of x-hat. + * @param value Value for element of x-hat. + */ + void SetXhat(int i, double value) { m_xHat(i) = value; } + + /** + * Resets the observer. + */ + void Reset() { m_xHat.setZero(); } + + /** + * Project the model into the future with a new control input u. + * + * @param u New control input from controller. + * @param dt Timestep for prediction. + */ + void Predict(const Eigen::Matrix& u, units::second_t dt) { + m_xHat = m_plant->CalculateX(m_xHat, u, dt); + + Eigen::Matrix discA; + Eigen::Matrix discQ; + DiscretizeAQTaylor(m_plant->A(), m_contQ, dt, &discA, &discQ); + + m_P = discA * m_P * discA.transpose() + discQ; + m_discR = DiscretizeR(m_contR, dt); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * @param u Same control input used in the last predict step. + * @param y Measurement vector. + */ + void Correct(const Eigen::Matrix& u, + const Eigen::Matrix& y) { + Correct(u, y, m_plant->C(), m_plant->D(), m_discR); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * This is useful for when the measurements available during a timestep's + * Correct() call vary. The C matrix passed to the constructor is used if one + * is not provided (the two-argument version of this function). + * + * @param u Same control input used in the predict step. + * @param y Measurement vector. + * @param C Output matrix. + * @param D Feedthrough matrix. + * @param R Measurement noise covariance matrix. + */ + template + void Correct(const Eigen::Matrix& u, + const Eigen::Matrix& y, + const Eigen::Matrix& C, + const Eigen::Matrix& D, + const Eigen::Matrix& R) { + const auto& x = m_xHat; + Eigen::Matrix S = C * m_P * C.transpose() + R; + + // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more + // efficiently. + // + // K = PC^T S^-1 + // KS = PC^T + // (KS)^T = (PC^T)^T + // S^T K^T = CP^T + // + // The solution of Ax = b can be found via x = A.solve(b). + // + // K^T = S^T.solve(CP^T) + // K = (S^T.solve(CP^T))^T + Eigen::Matrix K = + S.transpose().ldlt().solve(C * m_P.transpose()).transpose(); + + m_xHat = x + K * (y - (C * x + D * u)); + m_P = (Eigen::Matrix::Identity() - K * C) * m_P; + } + + private: + LinearSystem* m_plant; + + /** + * Error covariance matrix. + */ + Eigen::Matrix m_P; + + /** + * Continuous process noise covariance matrix. + */ + Eigen::Matrix m_contQ; + + /** + * Continuous measurement noise covariance matrix. + */ + Eigen::Matrix m_contR; + + /** + * Discrete measurement noise covariance matrix. + */ + Eigen::Matrix m_discR; + + /** + * State estimate x-hat. + */ + Eigen::Matrix m_xHat; +}; + +} // namespace detail + +template +class KalmanFilter : public detail::KalmanFilterImpl { + public: + /** + * Constructs a state-space observer with the given plant. + * + * @param plant The plant used for the prediction step. + * @param stateStdDevs Standard deviations of model states. + * @param measurementStdDevs Standard deviations of measurements. + * @param dt Nominal discretization timestep. + */ + KalmanFilter(LinearSystem& plant, + const std::array& stateStdDevs, + const std::array& measurementStdDevs, + units::second_t dt) + : detail::KalmanFilterImpl{ + plant, stateStdDevs, measurementStdDevs, dt} {} + + KalmanFilter(KalmanFilter&&) = default; + KalmanFilter& operator=(KalmanFilter&&) = default; +}; + +// Template specializations are used here to make common state-input-output +// triplets compile faster. +template <> +class KalmanFilter<1, 1, 1> : public detail::KalmanFilterImpl<1, 1, 1> { + public: + KalmanFilter(LinearSystem<1, 1, 1>& plant, + const std::array& stateStdDevs, + const std::array& measurementStdDevs, + units::second_t dt); + + KalmanFilter(KalmanFilter&&) = default; + KalmanFilter& operator=(KalmanFilter&&) = default; +}; + +// Template specializations are used here to make common state-input-output +// triplets compile faster. +template <> +class KalmanFilter<2, 1, 1> : public detail::KalmanFilterImpl<2, 1, 1> { + public: + KalmanFilter(LinearSystem<2, 1, 1>& plant, + const std::array& stateStdDevs, + const std::array& measurementStdDevs, + units::second_t dt); + + KalmanFilter(KalmanFilter&&) = default; + KalmanFilter& operator=(KalmanFilter&&) = default; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h new file mode 100644 index 0000000000..62ef9bac5e --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h @@ -0,0 +1,134 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "Eigen/Cholesky" +#include "Eigen/Core" + +namespace frc { + +/** + * Generates sigma points and weights according to Van der Merwe's 2004 + * dissertation[1] for the UnscentedKalmanFilter class. + * + * It parametrizes the sigma points using alpha, beta, kappa terms, and is the + * version seen in most publications. Unless you know better, this should be + * your default choice. + * + * @tparam States The dimensionality of the state. 2*States+1 weights will be + * generated. + * + * [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic + * Inference in Dynamic State-Space Models" (Doctoral dissertation) + */ +template +class MerweScaledSigmaPoints { + public: + /** + * Constructs a generator for Van der Merwe scaled sigma points. + * + * @param alpha Determines the spread of the sigma points around the mean. + * Usually a small positive value (1e-3). + * @param beta Incorporates prior knowledge of the distribution of the mean. + * For Gaussian distributions, beta = 2 is optimal. + * @param kappa Secondary scaling parameter usually set to 0 or 3 - States. + */ + MerweScaledSigmaPoints(double alpha = 1e-3, double beta = 2, + int kappa = 3 - States) { + m_alpha = alpha; + m_kappa = kappa; + + ComputeWeights(beta); + } + + /** + * Returns number of sigma points for each variable in the state x. + */ + int NumSigmas() { return 2 * States + 1; } + + /** + * Computes the sigma points for an unscented Kalman filter given the mean + * (x) and covariance(P) of the filter. + * + * @param x An array of the means. + * @param P Covariance of the filter. + * + * @return Two dimensional array of sigma points. Each column contains all of + * the sigmas for one dimension in the problem space. Ordered by + * Xi_0, Xi_{1..n}, Xi_{n+1..2n}. + * + */ + Eigen::Matrix SigmaPoints( + const Eigen::Matrix& x, + const Eigen::Matrix& P) { + double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States; + Eigen::Matrix U = + ((lambda + States) * P).llt().matrixL(); + + Eigen::Matrix sigmas; + sigmas.template block(0, 0) = x; + for (int k = 0; k < States; ++k) { + sigmas.template block(0, k + 1) = + x + U.template block(0, k); + sigmas.template block(0, States + k + 1) = + x - U.template block(0, k); + } + + return sigmas; + } + + /** + * Returns the weight for each sigma point for the mean. + */ + const Eigen::Matrix& Wm() const { return m_Wm; } + + /** + * Returns an element of the weight for each sigma point for the mean. + * + * @param i Element of vector to return. + */ + double Wm(int i) const { return m_Wm(i, 0); } + + /** + * Returns the weight for each sigma point for the covariance. + */ + const Eigen::Matrix& Wc() const { return m_Wc; } + + /** + * Returns an element of the weight for each sigma point for the covariance. + * + * @param i Element of vector to return. + */ + double Wc(int i) const { return m_Wc(i, 0); } + + private: + Eigen::Matrix m_Wm; + Eigen::Matrix m_Wc; + double m_alpha; + int m_kappa; + + /** + * Computes the weights for the scaled unscented Kalman filter. + * + * @param beta Incorporates prior knowledge of the distribution of the mean. + */ + void ComputeWeights(double beta) { + double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States; + + double c = 0.5 / (States + lambda); + m_Wm = Eigen::Matrix::Constant(c); + m_Wc = Eigen::Matrix::Constant(c); + + m_Wm(0) = lambda / (States + lambda); + m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta); + } +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h new file mode 100644 index 0000000000..e69ae9196d --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h @@ -0,0 +1,233 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "Eigen/Cholesky" +#include "Eigen/Core" +#include "frc/StateSpaceUtil.h" +#include "frc/estimator/MerweScaledSigmaPoints.h" +#include "frc/estimator/UnscentedTransform.h" +#include "frc/system/Discretization.h" +#include "frc/system/NumericalJacobian.h" +#include "frc/system/RungeKutta.h" +#include "units/time.h" + +namespace frc { + +template +class UnscentedKalmanFilter { + public: + /** + * Constructs an unscented Kalman filter. + * + * @param f A vector-valued function of x and u that returns + * the derivative of the state vector. + * @param h A vector-valued function of x and u that returns + * the measurement vector. + * @param stateStdDevs Standard deviations of model states. + * @param measurementStdDevs Standard deviations of measurements. + * @param dt Nominal discretization timestep. + */ + UnscentedKalmanFilter(std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + f, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + h, + const std::array& stateStdDevs, + const std::array& measurementStdDevs, + units::second_t dt) + : m_f(f), m_h(h) { + m_contQ = MakeCovMatrix(stateStdDevs); + m_contR = MakeCovMatrix(measurementStdDevs); + + m_discR = DiscretizeR(m_contR, dt); + + Reset(); + } + + /** + * Returns the error covariance matrix P. + */ + const Eigen::Matrix& P() const { return m_P; } + + /** + * Returns an element of the error covariance matrix P. + * + * @param i Row of P. + * @param j Column of P. + */ + double P(int i, int j) const { return m_P(i, j); } + + /** + * Set the current error covariance matrix P. + * + * @param P The error covariance matrix P. + */ + void SetP(const Eigen::Matrix& P) { m_P = P; } + + /** + * Returns the state estimate x-hat. + */ + const Eigen::Matrix& Xhat() const { return m_xHat; } + + /** + * Returns an element of the state estimate x-hat. + * + * @param i Row of x-hat. + */ + double Xhat(int i) const { return m_xHat(i, 0); } + + /** + * Set initial state estimate x-hat. + * + * @param xHat The state estimate x-hat. + */ + void SetXhat(const Eigen::Matrix& xHat) { m_xHat = xHat; } + + /** + * Set an element of the initial state estimate x-hat. + * + * @param i Row of x-hat. + * @param value Value for element of x-hat. + */ + void SetXhat(int i, double value) { m_xHat(i, 0) = value; } + + /** + * Resets the observer. + */ + void Reset() { + m_xHat.setZero(); + m_P.setZero(); + m_sigmasF.setZero(); + } + + /** + * Project the model into the future with a new control input u. + * + * @param u New control input from controller. + * @param dt Timestep for prediction. + */ + void Predict(const Eigen::Matrix& u, units::second_t dt) { + // Discretize Q before projecting mean and covariance forward + Eigen::Matrix contA = + NumericalJacobianX(m_f, m_xHat, u); + Eigen::Matrix discA; + Eigen::Matrix discQ; + DiscretizeAQTaylor(contA, m_contQ, dt, &discA, &discQ); + + Eigen::Matrix sigmas = + m_pts.SigmaPoints(m_xHat, m_P); + + for (int i = 0; i < m_pts.NumSigmas(); ++i) { + Eigen::Matrix x = + sigmas.template block(0, i); + m_sigmasF.template block(0, i) = RungeKutta(m_f, x, u, dt); + } + + auto ret = + UnscentedTransform(m_sigmasF, m_pts.Wm(), m_pts.Wc()); + m_xHat = std::get<0>(ret); + m_P = std::get<1>(ret); + + m_P += discQ; + m_discR = DiscretizeR(m_contR, dt); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * @param u Same control input used in the predict step. + * @param y Measurement vector. + */ + void Correct(const Eigen::Matrix& u, + const Eigen::Matrix& y) { + Correct(u, y, m_h, m_discR); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * This is useful for when the measurements available during a timestep's + * Correct() call vary. The h(x, u) passed to the constructor is used if one + * is not provided (the two-argument version of this function). + * + * @param u Same control input used in the predict step. + * @param y Measurement vector. + * @param h A vector-valued function of x and u that returns + * the measurement vector. + * @param R Measurement noise covariance matrix. + */ + template + void Correct(const Eigen::Matrix& u, + const Eigen::Matrix& y, + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + h, + const Eigen::Matrix& R) { + // Transform sigma points into measurement space + Eigen::Matrix sigmasH; + Eigen::Matrix sigmas = + m_pts.SigmaPoints(m_xHat, m_P); + for (int i = 0; i < m_pts.NumSigmas(); ++i) { + sigmasH.template block(0, i) = + h(sigmas.template block(0, i), u); + } + + // Mean and covariance of prediction passed through UT + auto [yHat, Py] = + UnscentedTransform(sigmasH, m_pts.Wm(), m_pts.Wc()); + Py += R; + + // Compute cross covariance of the state and the measurements + Eigen::Matrix Pxy; + Pxy.setZero(); + for (int i = 0; i < m_pts.NumSigmas(); ++i) { + Pxy += m_pts.Wc(i) * + (m_sigmasF.template block(0, i) - m_xHat) * + (sigmasH.template block(0, i) - yHat).transpose(); + } + + // K = P_{xy} Py^-1 + // K^T = P_y^T^-1 P_{xy}^T + // P_y^T K^T = P_{xy}^T + // K^T = P_y^T.solve(P_{xy}^T) + // K = (P_y^T.solve(P_{xy}^T)^T + Eigen::Matrix K = + Py.transpose().ldlt().solve(Pxy.transpose()).transpose(); + + m_xHat += K * (y - yHat); + m_P -= K * Py * K.transpose(); + } + + private: + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + m_f; + std::function( + const Eigen::Matrix&, + const Eigen::Matrix&)> + m_h; + Eigen::Matrix m_xHat; + Eigen::Matrix m_P; + Eigen::Matrix m_contQ; + Eigen::Matrix m_contR; + Eigen::Matrix m_discR; + Eigen::Matrix m_sigmasF; + + MerweScaledSigmaPoints m_pts; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h new file mode 100644 index 0000000000..22b32cedad --- /dev/null +++ b/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h @@ -0,0 +1,55 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include + +#include "Eigen/Core" + +namespace frc { + +/** + * Computes unscented transform of a set of sigma points and weights. CovDimurns + * the mean and covariance in a tuple. + * + * This works in conjunction with the UnscentedKalmanFilter class. + * + * @tparam States Number of states. + * @tparam CovDim Dimension of covariance of sigma points after passing through + * the transform. + * @param sigmas List of sigma points. + * @param Wm Weights for the mean. + * @param Wc Weights for the covariance. + * + * @return Tuple of x, mean of sigma points; P, covariance of sigma points after + * passing through the transform. + */ +template +std::tuple, + Eigen::Matrix> +UnscentedTransform(const Eigen::Matrix& sigmas, + const Eigen::Matrix& Wm, + const Eigen::Matrix& Wc) { + // New mean is just the sum of the sigmas * weight + // dot = \Sigma^n_1 (W[k]*Xi[k]) + Eigen::Matrix x = sigmas * Wm; + + // New covariance is the sum of the outer product of the residuals times the + // weights + Eigen::Matrix y; + for (int i = 0; i < 2 * States + 1; ++i) { + y.template block(0, i) = + sigmas.template block(0, i) - x; + } + Eigen::Matrix P = + y * Eigen::DiagonalMatrix(Wc) * y.transpose(); + + return std::make_tuple(x, P); +} + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/system/Discretization.h b/wpimath/src/main/native/include/frc/system/Discretization.h new file mode 100644 index 0000000000..8910dd2a46 --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/Discretization.h @@ -0,0 +1,165 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "Eigen/Core" +#include "units/time.h" +#include "unsupported/Eigen/MatrixFunctions" + +namespace frc { + +/** + * Discretizes the given continuous A matrix. + * + * @param contA Continuous system matrix. + * @param dt Discretization timestep. + * @param discA Storage for discrete system matrix. + */ +template +void DiscretizeA(const Eigen::Matrix& contA, + units::second_t dt, + Eigen::Matrix* discA) { + *discA = (contA * dt.to()).exp(); +} + +/** + * Discretizes the given continuous A and B matrices. + * + * @param contA Continuous system matrix. + * @param contB Continuous input matrix. + * @param dt Discretization timestep. + * @param discA Storage for discrete system matrix. + * @param discB Storage for discrete input matrix. + */ +template +void DiscretizeAB(const Eigen::Matrix& contA, + const Eigen::Matrix& contB, + units::second_t dt, + Eigen::Matrix* discA, + Eigen::Matrix* discB) { + // Matrices are blocked here to minimize matrix exponentiation calculations + Eigen::Matrix Mcont; + Mcont.setZero(); + Mcont.template block(0, 0) = contA * dt.to(); + Mcont.template block(0, States) = contB * dt.to(); + + // Discretize A and B with the given timestep + Eigen::Matrix Mdisc = Mcont.exp(); + *discA = Mdisc.template block(0, 0); + *discB = Mdisc.template block(0, States); +} + +/** + * Discretizes the given continuous A and Q matrices. + * + * @param contA Continuous system matrix. + * @param contQ Continuous process noise covariance matrix. + * @param dt Discretization timestep. + * @param discA Storage for discrete system matrix. + * @param discQ Storage for discrete process noise covariance matrix. + */ +template +void DiscretizeAQ(const Eigen::Matrix& contA, + const Eigen::Matrix& contQ, + units::second_t dt, + Eigen::Matrix* discA, + Eigen::Matrix* discQ) { + // Make continuous Q symmetric if it isn't already + Eigen::Matrix Q = (contQ + contQ.transpose()) / 2.0; + + // Set up the matrix M = [[-A, Q], [0, A.T]] + Eigen::Matrix M; + M.template block(0, 0) = -contA; + M.template block(0, States) = Q; + M.template block(States, 0).setZero(); + M.template block(States, States) = contA.transpose(); + + Eigen::Matrix phi = + (M * dt.to()).exp(); + + // Phi12 = phi[0:States, States:2*States] + // Phi22 = phi[States:2*States, States:2*States] + Eigen::Matrix phi12 = + phi.block(0, States, States, States); + Eigen::Matrix phi22 = + phi.block(States, States, States, States); + + *discA = phi22.transpose(); + + Q = *discA * phi12; + + // Make discrete Q symmetric if it isn't already + *discQ = (Q + Q.transpose()) / 2.0; +} + +/** + * Discretizes the given continuous A and Q matrices. + * + * Rather than solving a 2N x 2N matrix exponential like in DiscretizeAQ() + * (which is expensive), we take advantage of the structure of the block matrix + * of A and Q. + * + * 1) The exponential of A*t, which is only N x N, is relatively cheap. + * 2) The upper-right quarter of the 2N x 2N matrix, which we can approximate + * using a taylor series to several terms and still be substantially cheaper + * than taking the big exponential. + * + * @param contA Continuous system matrix. + * @param contQ Continuous process noise covariance matrix. + * @param dt Discretization timestep. + * @param discA Storage for discrete system matrix. + * @param discQ Storage for discrete process noise covariance matrix. + */ +template +void DiscretizeAQTaylor(const Eigen::Matrix& contA, + const Eigen::Matrix& contQ, + units::second_t dt, + Eigen::Matrix* discA, + Eigen::Matrix* discQ) { + // Make continuous Q symmetric if it isn't already + Eigen::Matrix Q = (contQ + contQ.transpose()) / 2.0; + + Eigen::Matrix lastTerm = Q; + double lastCoeff = dt.to(); + + // A^T^n + Eigen::Matrix Atn = contA.transpose(); + + Eigen::Matrix phi12 = lastTerm * lastCoeff; + + // i = 6 i.e. 5th order should be enough precision + for (int i = 2; i < 6; ++i) { + lastTerm = -contA * lastTerm + Q * Atn; + lastCoeff *= dt.to() / static_cast(i); + + phi12 += lastTerm * lastCoeff; + + Atn *= contA.transpose(); + } + + DiscretizeA(contA, dt, discA); + Q = *discA * phi12; + + // Make discrete Q symmetric if it isn't already + *discQ = (Q + Q.transpose()) / 2.0; +} + +/** + * Returns a discretized version of the provided continuous measurement noise + * covariance matrix. + * + * @param R Continuous measurement noise covariance matrix. + * @param dt Discretization timestep. + */ +template +Eigen::Matrix DiscretizeR( + const Eigen::Matrix& R, units::second_t dt) { + return R / dt.to(); +} + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/system/LinearSystem.h b/wpimath/src/main/native/include/frc/system/LinearSystem.h new file mode 100644 index 0000000000..7e4d8fec8b --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/LinearSystem.h @@ -0,0 +1,164 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include +#include + +#include "Eigen/Core" +#include "frc/StateSpaceUtil.h" +#include "frc/system/Discretization.h" +#include "units/time.h" + +namespace frc { + +/** + * A plant defined using state-space notation. + * + * A plant is a mathematical model of a system's dynamics. + * + * For more on the underlying math, read + * https://file.tavsys.net/control/state-space-guide.pdf. + */ +template +class LinearSystem { + public: + /** + * Constructs a discrete plant with the given continuous system coefficients. + * + * @param A System matrix. + * @param B Input matrix. + * @param C Output matrix. + * @param D Feedthrough matrix. + */ + LinearSystem(const Eigen::Matrix& A, + const Eigen::Matrix& B, + const Eigen::Matrix& C, + const Eigen::Matrix& D) { + m_A = A; + m_B = B; + m_C = C; + m_D = D; + } + + LinearSystem(const LinearSystem&) = default; + LinearSystem& operator=(const LinearSystem&) = default; + LinearSystem(LinearSystem&&) = default; + LinearSystem& operator=(LinearSystem&&) = default; + + /** + * Returns the system matrix A. + */ + const Eigen::Matrix& A() const { return m_A; } + + /** + * Returns an element of the system matrix A. + * + * @param i Row of A. + * @param j Column of A. + */ + double A(int i, int j) const { return m_A(i, j); } + + /** + * Returns the input matrix B. + */ + const Eigen::Matrix& B() const { return m_B; } + + /** + * Returns an element of the input matrix B. + * + * @param i Row of B. + * @param j Column of B. + */ + double B(int i, int j) const { return m_B(i, j); } + + /** + * Returns the output matrix C. + */ + const Eigen::Matrix& C() const { return m_C; } + + /** + * Returns an element of the output matrix C. + * + * @param i Row of C. + * @param j Column of C. + */ + double C(int i, int j) const { return m_C(i, j); } + + /** + * Returns the feedthrough matrix D. + */ + const Eigen::Matrix& D() const { return m_D; } + + /** + * Returns an element of the feedthrough matrix D. + * + * @param i Row of D. + * @param j Column of D. + */ + double D(int i, int j) const { return m_D(i, j); } + + /** + * Computes the new x given the old x and the control input. + * + * This is used by state observers directly to run updates based on state + * estimate. + * + * @param x The current state. + * @param u The control input. + * @param dt Timestep for model update. + */ + Eigen::Matrix CalculateX( + const Eigen::Matrix& x, + const Eigen::Matrix& clampedU, + units::second_t dt) const { + Eigen::Matrix discA; + Eigen::Matrix discB; + DiscretizeAB(m_A, m_B, dt, &discA, &discB); + + return discA * x + discB * clampedU; + } + + /** + * Computes the new y given the control input. + * + * This is used by state observers directly to run updates based on state + * estimate. + * + * @param x The current state. + * @param clampedU The control input. + */ + Eigen::Matrix CalculateY( + const Eigen::Matrix& x, + const Eigen::Matrix& clampedU) const { + return m_C * x + m_D * clampedU; + } + + private: + /** + * Continuous system matrix. + */ + Eigen::Matrix m_A; + + /** + * Continuous input matrix. + */ + Eigen::Matrix m_B; + + /** + * Output matrix. + */ + Eigen::Matrix m_C; + + /** + * Feedthrough matrix. + */ + Eigen::Matrix m_D; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h new file mode 100644 index 0000000000..229b24e25a --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h @@ -0,0 +1,262 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "Eigen/Core" +#include "frc/controller/LinearPlantInversionFeedforward.h" +#include "frc/controller/LinearQuadraticRegulator.h" +#include "frc/estimator/KalmanFilter.h" +#include "frc/system/LinearSystem.h" +#include "units/time.h" +#include "units/voltage.h" + +namespace frc { + +/** + * Combines a plant, controller, and observer for controlling a mechanism with + * full state feedback. + * + * For everything in this file, "inputs" and "outputs" are defined from the + * perspective of the plant. This means U is an input and Y is an output + * (because you give the plant U (powers) and it gives you back a Y (sensor + * values). This is the opposite of what they mean from the perspective of the + * controller (U is an output because that's what goes to the motors and Y is an + * input because that's what comes back from the sensors). + * + * For more on the underlying math, read + * https://file.tavsys.net/control/state-space-guide.pdf. + */ +template +class LinearSystemLoop { + public: + /** + * Constructs a state-space loop with the given plant, controller, and + * observer. By default, the initial reference is all zeros. Users should + * call reset with the initial system state before enabling the loop. + * + * @param plant State-space plant. + * @param controller State-space controller. + * @param feedforward Plant inversion feedforward. + * @param observer State-space observer. + * @param maxVoltageVolts The maximum voltage that can be applied. Assumes + * that the inputs are voltages. + */ + LinearSystemLoop(LinearSystem& plant, + LinearQuadraticRegulator& controller, + LinearPlantInversionFeedforward& feedforward, + KalmanFilter& observer, + units::volt_t maxVoltage) + : LinearSystemLoop(plant, controller, feedforward, observer, + [=](Eigen::Matrix u) { + return frc::NormalizeInputVector( + u, maxVoltage.template to()); + }) {} + + /** + * Constructs a state-space loop with the given plant, controller, and + * observer. + * + * @param plant State-space plant. + * @param controller State-space controller. + * @param feedforward Plant-inversion feedforward. + * @param observer State-space observer. + */ + LinearSystemLoop(LinearSystem& plant, + LinearQuadraticRegulator& controller, + LinearPlantInversionFeedforward& feedforward, + KalmanFilter& observer, + std::function( + const Eigen::Matrix&)> + clampFunction) + : m_plant(plant), + m_controller(controller), + m_feedforward(feedforward), + m_observer(observer), + m_clampFunc(clampFunction) { + m_nextR.setZero(); + Reset(m_nextR); + } + + virtual ~LinearSystemLoop() = default; + + LinearSystemLoop(LinearSystemLoop&&) = default; + LinearSystemLoop& operator=(LinearSystemLoop&&) = default; + + /** + * Returns the observer's state estimate x-hat. + */ + const Eigen::Matrix& Xhat() const { + return m_observer.Xhat(); + } + + /** + * Returns an element of the observer's state estimate x-hat. + * + * @param i Row of x-hat. + */ + double Xhat(int i) const { return m_observer.Xhat(i); } + + /** + * Returns the controller's next reference r. + */ + const Eigen::Matrix& NextR() const { return m_nextR; } + + /** + * Returns an element of the controller's next reference r. + * + * @param i Row of r. + */ + double NextR(int i) const { return NextR()(i); } + + /** + * Returns the controller's calculated control input u. + */ + Eigen::Matrix U() const { + return ClampInput(m_controller.U() + m_feedforward.Uff()); + } + + /** + * Returns an element of the controller's calculated control input u. + * + * @param i Row of u. + */ + double U(int i) const { return U()(i); } + + /** + * Set the initial state estimate x-hat. + * + * @param xHat The initial state estimate x-hat. + */ + void SetXhat(const Eigen::Matrix& xHat) { + m_observer.SetXhat(xHat); + } + + /** + * Set an element of the initial state estimate x-hat. + * + * @param i Row of x-hat. + * @param value Value for element of x-hat. + */ + void SetXhat(int i, double value) { m_observer.SetXhat(i, value); } + + /** + * Set the next reference r. + * + * @param nextR Next reference. + */ + void SetNextR(const Eigen::Matrix& nextR) { + m_nextR = nextR; + } + + /** + * Return the plant used internally. + */ + const LinearSystem& Plant() const { return m_plant; } + + /** + * Return the controller used internally. + */ + const LinearQuadraticRegulator& Controller() const { + return m_controller; + } + + /** + * Return the feedforward used internally. + * + * @return the feedforward used internally. + */ + const LinearPlantInversionFeedforward Feedforward() const { + return m_feedforward; + } + + /** + * Return the observer used internally. + */ + const KalmanFilter& Observer() const { + return m_observer; + } + + /** + * Zeroes reference r, controller output u and plant output y. + * The previous reference for PlantInversionFeedforward is set to the + * initial reference. + * @param initialReference The initial reference. + */ + void Reset(Eigen::Matrix initialState) { + m_controller.Reset(); + m_feedforward.Reset(initialState); + m_observer.Reset(); + m_nextR.setZero(); + } + + /** + * Returns difference between reference r and x-hat. + */ + const Eigen::Matrix Error() const { + return m_controller.R() - m_observer.Xhat(); + } + + /** + * Correct the state estimate x-hat using the measurements in y. + * + * @param y Measurement vector. + */ + void Correct(const Eigen::Matrix& y) { + m_observer.Correct(U(), y); + } + + /** + * Sets new controller output, projects model forward, and runs observer + * prediction. + * + * After calling this, the user should send the elements of u to the + * actuators. + * + * @param dt Timestep for model update. + */ + void Predict(units::second_t dt) { + Eigen::Matrix u = + ClampInput(m_controller.Calculate(m_observer.Xhat(), m_nextR) + + m_feedforward.Calculate(m_nextR)); + m_observer.Predict(u, dt); + } + + /** + * Clamps input vector between system's minimum and maximum allowable input. + * + * @param u Input vector to clamp. + * @return Clamped input vector. + */ + Eigen::Matrix ClampInput( + const Eigen::Matrix& u) const { + return m_clampFunc(u); + } + + protected: + LinearSystem& m_plant; + LinearQuadraticRegulator& m_controller; + LinearPlantInversionFeedforward& m_feedforward; + KalmanFilter& m_observer; + + /** + * Clamping function. + */ + std::function( + const Eigen::Matrix&)> + m_clampFunc; + + // Reference to go to in the next cycle (used by feedforward controller). + Eigen::Matrix m_nextR; + + // These are accessible from non-templated subclasses. + static constexpr int kStates = States; + static constexpr int kInputs = Inputs; + static constexpr int kOutputs = Outputs; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/system/NumericalJacobian.h b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h new file mode 100644 index 0000000000..cbd6943f04 --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/NumericalJacobian.h @@ -0,0 +1,80 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "Eigen/Core" + +namespace frc { + +/** + * Returns numerical Jacobian with respect to x for f(x). + * + * @tparam Rows Number of rows in result of f(x). + * @tparam Cols Number of columns in result of f(x). + * @param f Vector-valued function from which to compute Jacobian. + * @param x Vector argument. + */ +template +auto NumericalJacobian(F&& f, const Eigen::Matrix& x) { + constexpr double kEpsilon = 1e-5; + Eigen::Matrix result; + result.setZero(); + + // It's more expensive, but +- epsilon will be more accurate + for (int i = 0; i < Cols; ++i) { + Eigen::Matrix dX_plus = x; + dX_plus(i) += kEpsilon; + Eigen::Matrix dX_minus = x; + dX_minus(i) -= kEpsilon; + result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0); + } + + return result; +} + +/** + * Returns numerical Jacobian with respect to x for f(x, u, ...). + * + * @tparam Rows Number of rows in result of f(x, u, ...). + * @tparam States Number of rows in x. + * @tparam Inputs Number of rows in u. + * @tparam F Function object type. + * @tparam Args... Remaining arguments to f(x, u, ...). + * @param f Vector-valued function from which to compute Jacobian. + * @param x State vector. + * @param u Input vector. + */ +template +auto NumericalJacobianX(F&& f, const Eigen::Matrix& x, + const Eigen::Matrix& u, + Args&&... args) { + return NumericalJacobian( + [&](Eigen::Matrix x) { return f(x, u, args...); }, x); +} + +/** + * Returns numerical Jacobian with respect to u for f(x, u, ...). + * + * @tparam Rows Number of rows in result of f(x, u, ...). + * @tparam States Number of rows in x. + * @tparam Inputs Number of rows in u. + * @tparam F Function object type. + * @tparam Args... Remaining arguments to f(x, u, ...). + * @param f Vector-valued function from which to compute Jacobian. + * @param x State vector. + * @param u Input vector. + */ +template +auto NumericalJacobianU(F&& f, const Eigen::Matrix& x, + const Eigen::Matrix& u, + Args&&... args) { + return NumericalJacobian( + [&](Eigen::Matrix u) { return f(x, u, args...); }, u); +} + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/system/RungeKutta.h b/wpimath/src/main/native/include/frc/system/RungeKutta.h new file mode 100644 index 0000000000..f54e558fc8 --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/RungeKutta.h @@ -0,0 +1,69 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "Eigen/Core" +#include "units/time.h" + +namespace frc { + +/** + * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt. + * + * @param f The function to integrate. It must take one argument x. + * @param x The initial value of x. + * @param dt The time over which to integrate. + */ +template +T RungeKutta(F&& f, T x, units::second_t dt) { + const auto halfDt = 0.5 * dt; + T k1 = f(x); + T k2 = f(x + k1 * halfDt.to()); + T k3 = f(x + k2 * halfDt.to()); + T k4 = f(x + k3 * dt.to()); + return x + dt.to() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4); +} + +/** + * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt. + * + * @param f The function to integrate. It must take two arguments x and u. + * @param x The initial value of x. + * @param u The value u held constant over the integration period. + * @param dt The time over which to integrate. + */ +template +T RungeKutta(F&& f, T x, U u, units::second_t dt) { + const auto halfDt = 0.5 * dt; + T k1 = f(x, u); + T k2 = f(x + k1 * halfDt.to(), u); + T k3 = f(x + k2 * halfDt.to(), u); + T k4 = f(x + k3 * dt.to(), u); + return x + dt.to() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4); +} + +/** + * Performs 4th order Runge-Kutta integration of dx/dt = f(t, x) for dt. + * + * @param f The function to integrate. It must take two arguments x and t. + * @param x The initial value of x. + * @param t The intial value of t. + * @param dt The time over which to integrate. + */ +template +T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) { + const auto halfDt = 0.5 * dt; + T k1 = f(t, x); + T k2 = f(t + halfDt, x + k1 / 2.0); + T k3 = f(t + halfDt, x + k2 / 2.0); + T k4 = f(t + dt, x + k3); + + return x + dt.to() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4); +} + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/system/plant/DCMotor.h b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h new file mode 100644 index 0000000000..d2a2ba8910 --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/plant/DCMotor.h @@ -0,0 +1,157 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "units/angular_velocity.h" +#include "units/current.h" +#include "units/impedance.h" +#include "units/torque.h" +#include "units/voltage.h" + +namespace frc { + +/** + * Holds the constants for a DC motor. + */ +class DCMotor { + public: + using radians_per_second_per_volt_t = + units::unit_t>>; + using newton_meters_per_ampere_t = + units::unit_t>>; + + units::volt_t nominalVoltage; + units::newton_meter_t stallTorque; + units::ampere_t stallCurrent; + units::ampere_t freeCurrent; + units::radians_per_second_t freeSpeed; + + // Resistance of motor + units::ohm_t R; + + // Motor velocity constant + radians_per_second_per_volt_t Kv; + + // Torque constant + newton_meters_per_ampere_t Kt; + + /** + * Constructs a DC motor. + * + * @param nominalVoltage Voltage at which the motor constants were measured. + * @param stallTorque Current draw when stalled. + * @param stallCurrent Current draw when stalled. + * @param freeCurrent Current draw under no load. + * @param freeSpeed Angular velocity under no load. + * @param numMotors Number of motors in a gearbox. + */ + constexpr DCMotor(units::volt_t nominalVoltage, + units::newton_meter_t stallTorque, + units::ampere_t stallCurrent, units::ampere_t freeCurrent, + units::radians_per_second_t freeSpeed, int numMotors = 1) + : nominalVoltage(nominalVoltage), + stallTorque(stallTorque * numMotors), + stallCurrent(stallCurrent), + freeCurrent(freeCurrent), + freeSpeed(freeSpeed), + R(nominalVoltage / stallCurrent), + Kv(freeSpeed / (nominalVoltage - R * freeCurrent)), + Kt(stallTorque * numMotors / stallCurrent) {} + + /** + * Returns current drawn by motor with given speed and input voltage. + * + * @param speed The current angular velocity of the motor. + * @param inputVoltage The voltage being applied to the motor. + */ + constexpr units::ampere_t Current(units::radians_per_second_t speed, + units::volt_t inputVoltage) const { + return -1.0 / Kv / R * speed + 1.0 / R * inputVoltage; + } + + /** + * Returns instance of CIM. + */ + static constexpr DCMotor CIM(int numMotors = 1) { + return DCMotor(12_V, 2.42_Nm, 133_A, 2.7_A, 5310_rpm, numMotors); + } + + /** + * Returns instance of MiniCIM. + */ + static constexpr DCMotor MiniCIM(int numMotors = 1) { + return DCMotor(12_V, 1.41_Nm, 89_A, 3_A, 5840_rpm, numMotors); + } + + /** + * Returns instance of Bag motor. + */ + static constexpr DCMotor Bag(int numMotors = 1) { + return DCMotor(12_V, 0.43_Nm, 53_A, 1.8_A, 13180_rpm, numMotors); + } + + /** + * Returns instance of Vex 775 Pro. + */ + static constexpr DCMotor Vex775Pro(int numMotors = 1) { + return DCMotor(12_V, 0.71_Nm, 134_A, 0.7_A, 18730_rpm, numMotors); + } + + /** + * Returns instance of Andymark RS 775-125. + */ + static constexpr DCMotor RS775_125(int numMotors = 1) { + return DCMotor(12_V, 0.28_Nm, 18_A, 1.6_A, 5800_rpm, numMotors); + } + + /** + * Returns instance of Banebots RS 775. + */ + static constexpr DCMotor BanebotsRS775(int numMotors = 1) { + return DCMotor(12_V, 0.72_Nm, 97_A, 2.7_A, 13050_rpm, numMotors); + } + + /** + * Returns instance of Andymark 9015. + */ + static constexpr DCMotor Andymark9015(int numMotors = 1) { + return DCMotor(12_V, 0.36_Nm, 71_A, 3.7_A, 14270_rpm, numMotors); + } + + /** + * Returns instance of Banebots RS 550. + */ + static constexpr DCMotor BanebotsRS550(int numMotors = 1) { + return DCMotor(12_V, 0.38_Nm, 84_A, 0.4_A, 19000_rpm, numMotors); + } + + /** + * Returns instance of NEO brushless motor. + */ + static constexpr DCMotor NEO(int numMotors = 1) { + return DCMotor(12_V, 2.6_Nm, 105_A, 1.8_A, 5676_rpm, numMotors); + } + + /** + * Returns instance of NEO 550 brushless motor. + */ + static constexpr DCMotor NEO550(int numMotors = 1) { + return DCMotor(12_V, 0.97_Nm, 100_A, 1.4_A, 11000_rpm, numMotors); + } + + /** + * Returns instance of Falcon 500 brushless motor. + */ + static constexpr DCMotor Falcon500(int numMotors = 1) { + return DCMotor(12_V, 4.69_Nm, 257_A, 1.5_A, 6380_rpm, numMotors); + } +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h new file mode 100644 index 0000000000..620f0f0fbb --- /dev/null +++ b/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h @@ -0,0 +1,213 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "frc/StateSpaceUtil.h" +#include "frc/system/LinearSystem.h" +#include "frc/system/plant/DCMotor.h" +#include "units/moment_of_inertia.h" + +namespace frc { + +class LinearSystemId { + public: + /** + * Constructs the state-space model for an elevator. + * + * States: [[position], [velocity]] + * Inputs: [[voltage]] + * Outputs: [[position]] + * + * @param motor Instance of DCMotor. + * @param m Carriage mass. + * @param r Pulley radius. + * @param G Gear ratio from motor to carriage. + */ + static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor, + units::kilogram_t m, + units::meter_t r, double G) { + auto A = frc::MakeMatrix<2, 2>( + 0.0, 1.0, 0.0, + (-std::pow(G, 2) * motor.Kt / + (motor.R * units::math::pow<2>(r) * m * motor.Kv)) + .to()); + auto B = frc::MakeMatrix<2, 1>( + 0.0, (G * motor.Kt / (motor.R * r * m)).to()); + auto C = frc::MakeMatrix<1, 2>(1.0, 0.0); + auto D = frc::MakeMatrix<1, 1>(0.0); + + return LinearSystem<2, 1, 1>(A, B, C, D); + } + + /** + * Constructs the state-space model for a single-jointed arm. + * + * States: [[angle], [angular velocity]] + * Inputs: [[voltage]] + * Outputs: [[angle]] + * + * @param motor Instance of DCMotor. + * @param J Moment of inertia. + * @param G Gear ratio from motor to carriage. + */ + static LinearSystem<2, 1, 1> SingleJointedArmSystem( + DCMotor motor, units::kilogram_square_meter_t J, double G) { + auto A = frc::MakeMatrix<2, 2>( + 0.0, 1.0, 0.0, + (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to()); + auto B = + frc::MakeMatrix<2, 1>(0.0, (G * motor.Kt / (motor.R * J)).to()); + auto C = frc::MakeMatrix<1, 2>(1.0, 0.0); + auto D = frc::MakeMatrix<1, 1>(0.0); + + return LinearSystem<2, 1, 1>(A, B, C, D); + } + + /** + * Constructs the state-space model for a 1 DOF velocity-only system from + * system identification data. + * + * States: [[velocity]] + * Inputs: [[voltage]] + * Outputs: [[velocity]] + * + * The parameters provided by the user are from this feedforward model: + * + * u = K_v v + K_a a + * + * @param kV The velocity gain, in volt seconds per distance. + * @param kA The acceleration gain, in volt seconds^2 per distance. + */ + static LinearSystem<1, 1, 1> IdentifyVelocitySystem(double kV, double kA) { + auto A = frc::MakeMatrix<1, 1>(-kV / kA); + auto B = frc::MakeMatrix<1, 1>(1.0 / kA); + auto C = frc::MakeMatrix<1, 1>(1.0); + auto D = frc::MakeMatrix<1, 1>(0.0); + + return LinearSystem<1, 1, 1>(A, B, C, D); + } + + /** + * Constructs the state-space model for a 1 DOF position system from system + * identification data. + * + * States: [[position], [velocity]] + * Inputs: [[voltage]] + * Outputs: [[position]] + * + * The parameters provided by the user are from this feedforward model: + * + * u = K_v v + K_a a + * + * @param kV The velocity gain, in volt seconds per distance. + * @param kA The acceleration gain, in volt seconds^2 per distance. + */ + static LinearSystem<2, 1, 1> IdentifyPositionSystem(double kV, double kA) { + auto A = frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kV / kA); + auto B = frc::MakeMatrix<2, 1>(0.0, 1.0 / kA); + auto C = frc::MakeMatrix<1, 2>(1.0, 0.0); + auto D = frc::MakeMatrix<1, 1>(0.0); + + return LinearSystem<2, 1, 1>(A, B, C, D); + } + + /** + * Constructs the state-space model for a 2 DOF drivetrain velocity system + * from system identification data. + * + * States: [[left velocity], [right velocity]] + * Inputs: [[left voltage], [right voltage]] + * Outputs: [[left velocity], [right velocity]] + * + * @param kVlinear The linear velocity gain, in volt seconds per distance. + * @param kAlinear The linear acceleration gain, in volt seconds^2 per + * distance. + * @param kVangular The angular velocity gain, in volt seconds per angle. + * @param kAangular The angular acceleration gain, in volt seconds^2 per + * angle. + */ + static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(double kVlinear, + double kAlinear, + double kVangular, + double kAangular) { + double c = 0.5 / (kAlinear * kAangular); + double A1 = c * (-kAlinear * kVangular - kVlinear * kAangular); + double A2 = c * (kAlinear * kVangular - kVlinear * kAangular); + double B1 = c * (kAlinear + kAangular); + double B2 = c * (kAangular - kAlinear); + + auto A = frc::MakeMatrix<2, 2>(A1, A2, A2, A1); + auto B = frc::MakeMatrix<2, 2>(B1, B2, B2, B1); + auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0); + auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0); + + return LinearSystem<2, 2, 2>(A, B, C, D); + } + + /** + * Constructs the state-space model for a flywheel. + * + * States: [[angular velocity]] + * Inputs: [[voltage]] + * Outputs: [[angular velocity]] + * + * @param motor Instance of DCMotor. + * @param J Moment of inertia. + * @param G Gear ratio from motor to carriage. + */ + static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor, + units::kilogram_square_meter_t J, + double G) { + auto A = frc::MakeMatrix<1, 1>( + (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to()); + auto B = frc::MakeMatrix<1, 1>((G * motor.Kt / (motor.R * J)).to()); + auto C = frc::MakeMatrix<1, 1>(1.0); + auto D = frc::MakeMatrix<1, 1>(0.0); + + return LinearSystem<1, 1, 1>(A, B, C, D); + } + + /** + * Constructs the state-space model for a drivetrain. + * + * States: [[left velocity], [right velocity]] + * Inputs: [[left voltage], [right voltage]] + * Outputs: [[left velocity], [right velocity]] + * + * @param motor Instance of DCMotor. + * @param m Drivetrain mass. + * @param r Wheel radius. + * @param rb Robot radius. + * @param G Gear ratio from motor to wheel. + * @param J Moment of inertia. + */ + static LinearSystem<2, 2, 2> DrivetrainVelocitySystem( + DCMotor motor, units::kilogram_t m, units::meter_t r, units::meter_t rb, + units::kilogram_square_meter_t J, double G) { + auto C1 = -std::pow(G, 2) * motor.Kt / + (motor.Kv * motor.R * units::math::pow<2>(r)); + auto C2 = G * motor.Kt / (motor.R * r); + + auto A = frc::MakeMatrix<2, 2>( + ((1 / m + units::math::pow<2>(rb) / J) * C1).to(), + ((1 / m - units::math::pow<2>(rb) / J) * C1).to(), + ((1 / m - units::math::pow<2>(rb) / J) * C1).to(), + ((1 / m + units::math::pow<2>(rb) / J) * C1).to()); + auto B = frc::MakeMatrix<2, 2>( + ((1 / m + units::math::pow<2>(rb) / J) * C2).to(), + ((1 / m - units::math::pow<2>(rb) / J) * C2).to(), + ((1 / m - units::math::pow<2>(rb) / J) * C2).to(), + ((1 / m + units::math::pow<2>(rb) / J) * C2).to()); + auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0); + auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0); + + return LinearSystem<2, 2, 2>(A, B, C, D); + } +}; + +} // namespace frc diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java new file mode 100644 index 0000000000..18182d0b53 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java @@ -0,0 +1,74 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +class ControlAffinePlantInversionFeedforwardTest { + @SuppressWarnings("LocalVariableName") + @Test + void testCalculate() { + ControlAffinePlantInversionFeedforward feedforward = + new ControlAffinePlantInversionFeedforward( + Nat.N2(), + Nat.N1(), + this::getDynamics, + 0.02); + + assertEquals(48.0, feedforward.calculate( + VecBuilder.fill(2, 2), + VecBuilder.fill(3, 3)).get(0, 0), + 1e-6); + } + + @SuppressWarnings("LocalVariableName") + @Test + void testCalculateState() { + Matrix B = VecBuilder.fill(0, 1); + + ControlAffinePlantInversionFeedforward feedforward = + new ControlAffinePlantInversionFeedforward( + Nat.N2(), + Nat.N1(), + this::getStateDynamics, + B, + 0.02); + + assertEquals(48.0, feedforward.calculate( + VecBuilder.fill(2, 2), + VecBuilder.fill(3, 3)).get(0, 0), + 1e-6); + } + + @SuppressWarnings("ParameterName") + protected Matrix getDynamics(Matrix x, Matrix u) { + var result = new Matrix<>(Nat.N2(), Nat.N1()); + + result = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x) + .plus(VecBuilder.fill(0, 1).times(u)); + + return result; + } + + @SuppressWarnings("ParameterName") + protected Matrix getStateDynamics(Matrix x) { + var result = new Matrix<>(Nat.N2(), Nat.N1()); + + result = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x); + + return result; + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforwardTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforwardTest.java new file mode 100644 index 0000000000..8a09383f22 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforwardTest.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +class LinearPlantInversionFeedforwardTest { + @SuppressWarnings("LocalVariableName") + @Test + void testCalculate() { + Matrix A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1); + Matrix B = VecBuilder.fill(0, 1); + + LinearPlantInversionFeedforward feedforward = + new LinearPlantInversionFeedforward(A, B, 0.02); + + assertEquals(47.502599, feedforward.calculate( + VecBuilder.fill(2, 2), + VecBuilder.fill(3, 3)).get(0, 0), + 0.002); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java new file mode 100644 index 0000000000..e047198397 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java @@ -0,0 +1,116 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; + + +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class LinearQuadraticRegulatorTest { + public static LinearSystem elevatorPlant; + static LinearSystem armPlant; + + static { + createArm(); + createElevator(); + } + + @SuppressWarnings("LocalVariableName") + public static void createArm() { + var motors = DCMotor.getVex775Pro(2); + + var m = 4.0; + var r = 0.4; + var J = 1d / 3d * m * r * r; + var G = 100.0; + + armPlant = LinearSystemId.createSingleJointedArmSystem(motors, J, G); + } + + @SuppressWarnings("LocalVariableName") + public static void createElevator() { + var motors = DCMotor.getVex775Pro(2); + + var m = 5.0; + var r = 0.0181864; + var G = 1.0; + elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G); + } + + @Test + @SuppressWarnings("LocalVariableName") + public void testLQROnElevator() { + + var qElms = VecBuilder.fill(0.02, 0.4); + var rElms = VecBuilder.fill(12.0); + var dt = 0.00505; + + var controller = new LinearQuadraticRegulator<>( + elevatorPlant, qElms, rElms, dt); + + var k = controller.getK(); + + assertEquals(522.153, k.get(0, 0), 0.1); + assertEquals(38.2, k.get(0, 1), 0.1); + } + + @Test + public void testFourMotorElevator() { + + var dt = 0.020; + + var plant = LinearSystemId.createElevatorSystem( + DCMotor.getVex775Pro(4), + 8.0, + 0.75 * 25.4 / 1000.0, + 14.67); + + var regulator = new LinearQuadraticRegulator<>( + plant, + VecBuilder.fill(0.1, 0.2), + VecBuilder.fill(12.0), + dt); + + assertEquals(10.381, regulator.getK().get(0, 0), 1e-2); + assertEquals(0.6929, regulator.getK().get(0, 1), 1e-2); + + } + + @Test + @SuppressWarnings("LocalVariableName") + public void testLQROnArm() { + + var motors = DCMotor.getVex775Pro(2); + + var m = 4.0; + var r = 0.4; + var G = 100.0; + + var plant = LinearSystemId.createSingleJointedArmSystem(motors, 1d / 3d * m * r * r, G); + + var qElms = VecBuilder.fill(0.01745, 0.08726); + var rElms = VecBuilder.fill(12.0); + var dt = 0.00505; + + var controller = new LinearQuadraticRegulator<>( + plant, qElms, rElms, dt); + + var k = controller.getK(); + + assertEquals(19.16, k.get(0, 0), 0.1); + assertEquals(3.32, k.get(0, 1), 0.1); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java new file mode 100644 index 0000000000..14a0a9d696 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java @@ -0,0 +1,169 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.controller; + +import java.util.ArrayList; +import java.util.List; +import java.util.Random; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.estimator.KalmanFilter; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.LinearSystemLoop; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +public class LinearSystemLoopTest { + public static final double kDt = 0.00505; + private static final double kPositionStddev = 0.0001; + private static final Random random = new Random(); + private final LinearSystemLoop m_loop; + + @SuppressWarnings("LocalVariableName") + public LinearSystemLoopTest() { + LinearSystem plant = LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, + 0.0181864, 1.0); + KalmanFilter observer = new KalmanFilter<>(Nat.N2(), Nat.N1(), plant, + VecBuilder.fill(0.05, 1.0), + VecBuilder.fill(0.0001), kDt); + + var qElms = VecBuilder.fill(0.02, 0.4); + var rElms = VecBuilder.fill(12.0); + var dt = 0.00505; + + var controller = new LinearQuadraticRegulator<>( + plant, qElms, rElms, dt); + + m_loop = new LinearSystemLoop<>(plant, controller, observer, 12, dt); + } + + @SuppressWarnings("LocalVariableName") + private static void updateTwoState(LinearSystemLoop loop, double noise) { + Matrix y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus( + VecBuilder.fill(noise) + ); + + loop.correct(y); + loop.predict(kDt); + } + + @Test + @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"}) + public void testStateSpaceEnabled() { + + m_loop.reset(VecBuilder.fill(0, 0)); + Matrix references = VecBuilder.fill(2.0, 0.0); + var constraints = new TrapezoidProfile.Constraints(4, 3); + m_loop.setNextR(references); + + TrapezoidProfile profile; + TrapezoidProfile.State state; + for (int i = 0; i < 1000; i++) { + profile = new TrapezoidProfile( + constraints, new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)), + new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0)) + ); + state = profile.calculate(kDt); + m_loop.setNextR(VecBuilder.fill(state.position, state.velocity)); + + updateTwoState(m_loop, (random.nextGaussian()) * kPositionStddev); + var u = m_loop.getU(0); + + assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u); + } + + assertEquals(2.0, m_loop.getXHat(0), 0.05); + assertEquals(0.0, m_loop.getXHat(1), 0.5); + + } + + @Test + @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"}) + public void testFlywheelEnabled() { + + LinearSystem plant = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), + 0.00289, 1.0); + KalmanFilter observer = new KalmanFilter<>(Nat.N1(), Nat.N1(), plant, + VecBuilder.fill(1.0), + VecBuilder.fill(kPositionStddev), kDt); + + var qElms = VecBuilder.fill(9.0); + var rElms = VecBuilder.fill(12.0); + + var controller = new LinearQuadraticRegulator<>( + plant, qElms, rElms, kDt); + + var feedforward = new LinearPlantInversionFeedforward<>(plant, kDt); + + var loop = new LinearSystemLoop<>(plant, controller, feedforward, observer, 12); + + loop.reset(VecBuilder.fill(0.0)); + var references = VecBuilder.fill(3000 / 60d * 2 * Math.PI); + + loop.setNextR(references); + + List timeData = new ArrayList<>(); + List xHat = new ArrayList<>(); + List volt = new ArrayList<>(); + List reference = new ArrayList<>(); + List error = new ArrayList<>(); + + var time = 0.0; + while (time < 10) { + + if (time > 10) { + break; + } + + loop.setNextR(references); + + Matrix y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus( + VecBuilder.fill(random.nextGaussian() * kPositionStddev) + ); + + loop.correct(y); + loop.predict(kDt); + var u = loop.getU(0); + + assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u); + + xHat.add(loop.getXHat(0) / 2d / Math.PI * 60); + volt.add(u); + timeData.add(time); + reference.add(references.get(0, 0) / 2d / Math.PI * 60); + error.add(loop.getError(0) / 2d / Math.PI * 60); + time += kDt; + } + + // XYChart bigChart = new XYChartBuilder().build(); + // bigChart.addSeries("Xhat, RPM", timeData, xHat); + // bigChart.addSeries("Reference, RPM", timeData, reference); + // bigChart.addSeries("Error, RPM", timeData, error); + + // XYChart smolChart = new XYChartBuilder().build(); + // smolChart.addSeries("Volts, V", timeData, volt); + + // try { + // new SwingWrapper<>(List.of(bigChart, smolChart)).displayChartMatrix(); + // Thread.sleep(10000000); + // } catch (InterruptedException e) { e.printStackTrace(); } + + assertEquals(0.0, loop.getError(0), 0.1); + } + +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java new file mode 100644 index 0000000000..6c7cc924dc --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java @@ -0,0 +1,219 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.estimator; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpilibj.system.NumericalJacobian; +import edu.wpi.first.wpilibj.system.RungeKutta; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; +import edu.wpi.first.wpiutil.math.numbers.N3; +import edu.wpi.first.wpiutil.math.numbers.N5; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; + +@SuppressWarnings("ParameterName") +public class ExtendedKalmanFilterTest { + public static Matrix getDynamics(final Matrix x, final Matrix u) { + final var motors = DCMotor.getCIM(2); + + final var gr = 7.08; // Gear ratio + final var rb = 0.8382 / 2.0; // Wheelbase radius (track width) + final var r = 0.0746125; // Wheel radius + final var m = 63.503; // Robot mass + final var J = 5.6; // Robot moment of inertia + + final var C1 = + -Math.pow(gr, 2) * motors.m_KtNMPerAmp / ( + motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r); + final var C2 = gr * motors.m_KtNMPerAmp / (motors.m_rOhms * r); + final var k1 = 1.0 / m + rb * rb / J; + final var k2 = 1.0 / m - rb * rb / J; + + final var vl = x.get(3, 0); + final var vr = x.get(4, 0); + final var Vl = u.get(0, 0); + final var Vr = u.get(1, 0); + + final Matrix result = new Matrix<>(Nat.N5(), Nat.N1()); + final var v = 0.5 * (vl + vr); + result.set(0, 0, v * Math.cos(x.get(2, 0))); + result.set(1, 0, v * Math.sin(x.get(2, 0))); + result.set(2, 0, (vr - vl) / (2.0 * rb)); + result.set(3, 0, k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr))); + result.set(4, 0, k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr))); + return result; + } + + public static Matrix getLocalMeasurementModel(Matrix x, Matrix u) { + return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0)); + } + + public static Matrix getGlobalMeasurementModel(Matrix x, Matrix u) { + return VecBuilder.fill( + x.get(0, 0), + x.get(1, 0), + x.get(2, 0), + x.get(3, 0), + x.get(4, 0) + ); + } + + @SuppressWarnings("LocalVariableName") + @Test + public void testInit() { + double dtSeconds = 0.00505; + + assertDoesNotThrow(() -> { + ExtendedKalmanFilter observer = + new ExtendedKalmanFilter<>(Nat.N5(), Nat.N2(), Nat.N3(), + ExtendedKalmanFilterTest::getDynamics, + ExtendedKalmanFilterTest::getLocalMeasurementModel, + VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), + VecBuilder.fill(0.0001, 0.01, 0.01), dtSeconds); + + Matrix u = VecBuilder.fill(12.0, 12.0); + observer.predict(u, dtSeconds); + + var localY = getLocalMeasurementModel(observer.getXhat(), u); + observer.correct(u, localY); + + var globalY = getGlobalMeasurementModel(observer.getXhat(), u); + var R = StateSpaceUtil.makeCostMatrix( + VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5)); + observer.correct(Nat.N5(), + u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R); + }); + + } + + @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops", + "PMD.ExcessiveMethodLength"}) + @Test + public void testConvergence() { + double dtSeconds = 0.00505; + double rbMeters = 0.8382 / 2.0; // Robot radius + + ExtendedKalmanFilter observer = + new ExtendedKalmanFilter<>(Nat.N5(), Nat.N2(), Nat.N3(), + ExtendedKalmanFilterTest::getDynamics, + ExtendedKalmanFilterTest::getLocalMeasurementModel, + VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0), + VecBuilder.fill(0.001, 0.01, 0.01), dtSeconds); + + List waypoints = Arrays.asList(new Pose2d(2.75, 22.521, new Rotation2d()), + new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846))); + var trajectory = TrajectoryGenerator.generateTrajectory( + waypoints, + new TrajectoryConfig(8.8, 0.1) + ); + + Matrix r = new Matrix<>(Nat.N5(), Nat.N1()); + + Matrix nextR = new Matrix<>(Nat.N5(), Nat.N1()); + Matrix u = new Matrix<>(Nat.N2(), Nat.N1()); + + List trajXs = new ArrayList<>(); + List trajYs = new ArrayList<>(); + + List observerXs = new ArrayList<>(); + List observerYs = new ArrayList<>(); + + var B = NumericalJacobian.numericalJacobianU(Nat.N5(), Nat.N2(), + ExtendedKalmanFilterTest::getDynamics, new Matrix<>(Nat.N5(), Nat.N1()), u); + + observer.setXhat(VecBuilder.fill( + trajectory.getInitialPose().getTranslation().getX(), + trajectory.getInitialPose().getTranslation().getY(), + trajectory.getInitialPose().getRotation().getRadians(), + 0.0, 0.0)); + + var groundTruthX = observer.getXhat(); + + double totalTime = trajectory.getTotalTimeSeconds(); + for (int i = 0; i < (totalTime / dtSeconds); i++) { + var ref = trajectory.sample(dtSeconds * i); + double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)); + double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters)); + + nextR.set(0, 0, ref.poseMeters.getTranslation().getX()); + nextR.set(1, 0, ref.poseMeters.getTranslation().getY()); + nextR.set(2, 0, ref.poseMeters.getRotation().getRadians()); + nextR.set(3, 0, vl); + nextR.set(4, 0, vr); + + var localY = + getLocalMeasurementModel(groundTruthX, u); + var whiteNoiseStdDevs = VecBuilder.fill(0.0001, 0.5, 0.5); + observer.correct(u, + localY.plus(StateSpaceUtil.makeWhiteNoiseVector(whiteNoiseStdDevs))); + + Matrix rdot = nextR.minus(r).div(dtSeconds); + u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1()))))); + + observer.predict(u, dtSeconds); + + groundTruthX = RungeKutta.rungeKutta(ExtendedKalmanFilterTest::getDynamics, + groundTruthX, u, dtSeconds); + + r = nextR; + + trajXs.add(ref.poseMeters.getTranslation().getX()); + trajYs.add(ref.poseMeters.getTranslation().getY()); + + observerXs.add(observer.getXhat().get(0, 0)); + observerYs.add(observer.getXhat().get(1, 0)); + } + + var localY = getLocalMeasurementModel(observer.getXhat(), u); + observer.correct(u, localY); + + var globalY = getGlobalMeasurementModel(observer.getXhat(), u); + var R = StateSpaceUtil.makeCostMatrix( + VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5)); + observer.correct(Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R); + + // var chartBuilder = new XYChartBuilder(); + // chartBuilder.title = "The Magic of Sensor Fusion, now with a " + // + observer.getClass().getSimpleName(); + // var xyPosChart = chartBuilder.build(); + // + // xyPosChart.setXAxisTitle("X pos, meters"); + // xyPosChart.setYAxisTitle("Y pos, meters"); + // xyPosChart.addSeries("Trajectory", trajXs, trajYs); + // xyPosChart.addSeries("xHat", observerXs, observerYs); + // new SwingWrapper<>(xyPosChart).displayChart(); + // try { + // Thread.sleep(1000000000); + // } catch (InterruptedException ex) { + // ex.printStackTrace(); + // } + + var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds()); + assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 1.0); + assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 1.0); + assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0); + assertEquals(0.0, observer.getXhat(3), 1.0); + assertEquals(0.0, observer.getXhat(4), 1.0); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/KalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/KalmanFilterTest.java new file mode 100644 index 0000000000..2a434fb8c1 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/KalmanFilterTest.java @@ -0,0 +1,258 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.estimator; + +import java.util.ArrayList; +import java.util.List; +import java.util.Random; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.system.LinearSystem; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; +import edu.wpi.first.wpiutil.math.numbers.N3; +import edu.wpi.first.wpiutil.math.numbers.N6; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class KalmanFilterTest { + private static LinearSystem elevatorPlant; + + private static final double kDt = 0.00505; + + static { + createElevator(); + } + + @SuppressWarnings("LocalVariableName") + public static void createElevator() { + var motors = DCMotor.getVex775Pro(2); + + var m = 5.0; + var r = 0.0181864; + var G = 1.0; + elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G); + } + + // A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]^T, + // Y is [x, y, theta]^T and u is [ax, ay, alpha}^T + LinearSystem m_swerveObserverSystem = new LinearSystem<>( + Matrix.mat(Nat.N6(), Nat.N6()).fill( // A + 0, 0, 0, 1, 0, 0, + 0, 0, 0, 0, 1, 0, + 0, 0, 0, 0, 0, 1, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0), + Matrix.mat(Nat.N6(), Nat.N3()).fill( // B + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 1, 0, 0, + 0, 1, 0, + 0, 0, 1 + ), + Matrix.mat(Nat.N3(), Nat.N6()).fill( // C + 1, 0, 0, 0, 0, 0, + 0, 1, 0, 0, 0, 0, + 0, 0, 1, 0, 0, 0 + ), + new Matrix<>(Nat.N3(), Nat.N3())); // D + + @Test + @SuppressWarnings("LocalVariableName") + public void testElevatorKalmanFilter() { + + var Q = VecBuilder.fill(0.05, 1.0); + var R = VecBuilder.fill(0.0001); + + assertDoesNotThrow(() -> new KalmanFilter<>(Nat.N2(), Nat.N1(), elevatorPlant, Q, R, kDt)); + } + + @Test + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") + public void testSwerveKFStationary() { + + var random = new Random(); + + var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(), + m_swerveObserverSystem, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state + // weights + VecBuilder.fill(2, 2, 2), // measurement weights + 0.020 + ); + + List xhatsX = new ArrayList<>(); + List measurementsX = new ArrayList<>(); + List xhatsY = new ArrayList<>(); + List measurementsY = new ArrayList<>(); + + Matrix measurement; + for (int i = 0; i < 100; i++) { + // the robot is at [0, 0, 0] so we just park here + measurement = VecBuilder.fill( + random.nextGaussian(), random.nextGaussian(), random.nextGaussian()); + filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement); + + // we continue to not accelerate + filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020); + + measurementsX.add(measurement.get(0, 0)); + measurementsY.add(measurement.get(1, 0)); + xhatsX.add(filter.getXhat(0)); + xhatsY.add(filter.getXhat(1)); + } + + //var chart = new XYChartBuilder().build(); + //chart.addSeries("Xhat, x/y", xhatsX, xhatsY); + //chart.addSeries("Measured position, x/y", measurementsX, measurementsY); + //try { + // new SwingWrapper<>(chart).displayChart(); + // Thread.sleep(10000000); + //} catch (Exception ign) { + //} + assertEquals(0.0, filter.getXhat(0), 0.3); + assertEquals(0.0, filter.getXhat(0), 0.3); + } + + @Test + @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops") + public void testSwerveKFMovingWithoutAccelerating() { + + var random = new Random(); + + var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(), + m_swerveObserverSystem, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state + // weights + VecBuilder.fill(4, 4, 4), // measurement weights + 0.020 + ); + + List xhatsX = new ArrayList<>(); + List measurementsX = new ArrayList<>(); + List xhatsY = new ArrayList<>(); + List measurementsY = new ArrayList<>(); + + // we set the velocity of the robot so that it's moving forward slowly + filter.setXhat(0, 0.5); + filter.setXhat(1, 0.5); + + for (int i = 0; i < 300; i++) { + // the robot is at [0, 0, 0] so we just park here + var measurement = VecBuilder.fill( + random.nextGaussian() / 10d, + random.nextGaussian() / 10d, + random.nextGaussian() / 4d // std dev of [1, 1, 1] + ); + + filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement); + + // we continue to not accelerate + filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020); + + measurementsX.add(measurement.get(0, 0)); + measurementsY.add(measurement.get(1, 0)); + xhatsX.add(filter.getXhat(0)); + xhatsY.add(filter.getXhat(1)); + } + + //var chart = new XYChartBuilder().build(); + //chart.addSeries("Xhat, x/y", xhatsX, xhatsY); + //chart.addSeries("Measured position, x/y", measurementsX, measurementsY); + //try { + // new SwingWrapper<>(chart).displayChart(); + // Thread.sleep(10000000); + //} catch (Exception ign) {} + + assertEquals(0.0, filter.getXhat(0), 0.2); + assertEquals(0.0, filter.getXhat(1), 0.2); + } + + @Test + @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"}) + public void testSwerveKFMovingOverTrajectory() { + + var random = new Random(); + + var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(), + m_swerveObserverSystem, + VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state + // weights + VecBuilder.fill(4, 4, 4), // measurement weights + 0.020 + ); + + List xhatsX = new ArrayList<>(); + List measurementsX = new ArrayList<>(); + List xhatsY = new ArrayList<>(); + List measurementsY = new ArrayList<>(); + + var trajectory = TrajectoryGenerator.generateTrajectory( + List.of( + new Pose2d(0, 0, new Rotation2d()), + new Pose2d(5, 5, new Rotation2d()) + ), new TrajectoryConfig(2, 2) + ); + var time = 0.0; + var lastVelocity = VecBuilder.fill(0.0, 0.0, 0.0); + + while (time <= trajectory.getTotalTimeSeconds()) { + var sample = trajectory.sample(time); + var measurement = VecBuilder.fill( + sample.poseMeters.getTranslation().getX() + random.nextGaussian() / 5d, + sample.poseMeters.getTranslation().getY() + random.nextGaussian() / 5d, + sample.poseMeters.getRotation().getRadians() + random.nextGaussian() / 3d + ); + + var velocity = VecBuilder.fill( + sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getCos(), + sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getSin(), + sample.curvatureRadPerMeter * sample.velocityMetersPerSecond + ); + var u = (velocity.minus(lastVelocity)).div(0.020); + lastVelocity = velocity; + + filter.correct(u, measurement); + filter.predict(u, 0.020); + + measurementsX.add(measurement.get(0, 0)); + measurementsY.add(measurement.get(1, 0)); + xhatsX.add(filter.getXhat(0)); + xhatsY.add(filter.getXhat(1)); + + time += 0.020; + } + + //var chart = new XYChartBuilder().build(); + //chart.addSeries("Xhat, x/y", xhatsX, xhatsY); + //chart.addSeries("Measured position, x/y", measurementsX, measurementsY); + //try { + // new SwingWrapper<>(chart).displayChart(); + // Thread.sleep(10000000); + //} catch (Exception ign) { + //} + + assertEquals(trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters + .getTranslation().getX(), filter.getXhat(0), 0.2); + assertEquals(trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters + .getTranslation().getY(), filter.getXhat(1), 0.2); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPointsTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPointsTest.java new file mode 100644 index 0000000000..529a3c54e3 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPointsTest.java @@ -0,0 +1,42 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.estimator; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; + +import static org.junit.jupiter.api.Assertions.assertTrue; + +public class MerweScaledSigmaPointsTest { + @Test + public void testZeroMeanPoints() { + var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2()); + var points = merweScaledSigmaPoints.sigmaPoints(VecBuilder.fill(0, 0), + Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1)); + + assertTrue(points.isEqual(Matrix.mat(Nat.N2(), Nat.N5()).fill( + 0.0, 0.00173205, 0.0, -0.00173205, 0.0, + 0.0, 0.0, 0.00173205, 0.0, -0.00173205 + ), 1E-6)); + } + + @Test + public void testNonzeroMeanPoints() { + var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2()); + var points = merweScaledSigmaPoints.sigmaPoints(VecBuilder.fill(1, 2), + Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 10)); + + assertTrue(points.isEqual(Matrix.mat(Nat.N2(), Nat.N5()).fill( + 1.0, 1.00173205, 1.0, 0.99826795, 1.0, + 2.0, 2.0, 2.00547723, 2.0, 1.99452277 + ), 1E-6)); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java new file mode 100644 index 0000000000..c4340ae471 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java @@ -0,0 +1,396 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.estimator; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpilibj.math.Discretization; +import edu.wpi.first.wpilibj.math.StateSpaceUtil; +import edu.wpi.first.wpilibj.system.NumericalJacobian; +import edu.wpi.first.wpilibj.system.RungeKutta; +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; +import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; +import edu.wpi.first.wpiutil.math.numbers.N4; +import edu.wpi.first.wpiutil.math.numbers.N6; + +import static org.junit.jupiter.api.Assertions.assertDoesNotThrow; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + + +public class UnscentedKalmanFilterTest { + @SuppressWarnings({"LocalVariableName", "ParameterName"}) + public static Matrix getDynamics(Matrix x, Matrix u) { + var motors = DCMotor.getCIM(2); + + var gHigh = 7.08; + var rb = 0.8382 / 2.0; + var r = 0.0746125; + var m = 63.503; + var J = 5.6; + + var C1 = -Math.pow(gHigh, 2) * motors.m_KtNMPerAmp + / (motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r); + var C2 = gHigh * motors.m_KtNMPerAmp / (motors.m_rOhms * r); + + var c = x.get(2, 0); + var s = x.get(3, 0); + var vl = x.get(4, 0); + var vr = x.get(5, 0); + + var Vl = u.get(0, 0); + var Vr = u.get(1, 0); + + var k1 = 1.0 / m + rb * rb / J; + var k2 = 1.0 / m - rb * rb / J; + + var xvel = (vl + vr) / 2; + var w = (vr - vl) / (2.0 * rb); + + return VecBuilder.fill( + xvel * c, + xvel * s, + -s * w, + c * w, + k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)), + k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)) + ); + } + + @SuppressWarnings("ParameterName") + public static Matrix getLocalMeasurementModel(Matrix x, Matrix u) { + return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0), x.get(5, 0)); + } + + @SuppressWarnings("ParameterName") + public static Matrix getGlobalMeasurementModel(Matrix x, Matrix u) { + return x.copy(); + } + + @Test + @SuppressWarnings("LocalVariableName") + public void testInit() { + assertDoesNotThrow(() -> { + UnscentedKalmanFilter observer = new UnscentedKalmanFilter<>( + Nat.N6(), Nat.N4(), + UnscentedKalmanFilterTest::getDynamics, + UnscentedKalmanFilterTest::getLocalMeasurementModel, + VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0), + VecBuilder.fill(0.001, 0.001, 0.5, 0.5), + 0.00505); + + var u = VecBuilder.fill(12.0, 12.0); + observer.predict(u, 0.00505); + + var localY = getLocalMeasurementModel(observer.getXhat(), u); + observer.correct(u, localY); + }); + } + + @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops", + "PMD.ExcessiveMethodLength"}) + @Test + public void testConvergence() { + double dtSeconds = 0.00505; + double rbMeters = 0.8382 / 2.0; // Robot radius + + List trajXs = new ArrayList<>(); + List trajYs = new ArrayList<>(); + + List observerXs = new ArrayList<>(); + List observerYs = new ArrayList<>(); + List observerC = new ArrayList<>(); + List observerS = new ArrayList<>(); + List observervl = new ArrayList<>(); + List observervr = new ArrayList<>(); + + List inputVl = new ArrayList<>(); + List inputVr = new ArrayList<>(); + + List timeData = new ArrayList<>(); + List> rdots = new ArrayList<>(); + + UnscentedKalmanFilter observer = new UnscentedKalmanFilter<>( + Nat.N6(), Nat.N4(), + UnscentedKalmanFilterTest::getDynamics, + UnscentedKalmanFilterTest::getLocalMeasurementModel, + VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0), + VecBuilder.fill(0.001, 0.001, 0.5, 0.5), + dtSeconds); + + List waypoints = Arrays.asList(new Pose2d(2.75, 22.521, new Rotation2d()), + new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846))); + var trajectory = TrajectoryGenerator.generateTrajectory( + waypoints, + new TrajectoryConfig(8.8, 0.1) + ); + + Matrix nextR; + Matrix u = new Matrix<>(Nat.N2(), Nat.N1()); + + var B = NumericalJacobian.numericalJacobianU(Nat.N6(), Nat.N2(), + UnscentedKalmanFilterTest::getDynamics, new Matrix<>(Nat.N6(), Nat.N1()), u); + + observer.setXhat(VecBuilder.fill(2.75, 22.521, 1.0, 0.0, 0.0, 0.0)); // TODO not hard code this + + var ref = trajectory.sample(0.0); + + Matrix r = VecBuilder.fill( + ref.poseMeters.getTranslation().getX(), + ref.poseMeters.getTranslation().getY(), + ref.poseMeters.getRotation().getCos(), + ref.poseMeters.getRotation().getSin(), + ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)), + ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters)) + ); + nextR = r.copy(); + + var trueXhat = observer.getXhat(); + + double totalTime = trajectory.getTotalTimeSeconds(); + for (int i = 0; i < (totalTime / dtSeconds); i++) { + + ref = trajectory.sample(dtSeconds * i); + double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)); + double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters)); + + nextR.set(0, 0, ref.poseMeters.getTranslation().getX()); + nextR.set(1, 0, ref.poseMeters.getTranslation().getY()); + nextR.set(2, 0, ref.poseMeters.getRotation().getCos()); + nextR.set(3, 0, ref.poseMeters.getRotation().getSin()); + nextR.set(4, 0, vl); + nextR.set(5, 0, vr); + + Matrix localY = + getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1())); + var noiseStdDev = VecBuilder.fill(0.001, 0.001, 0.5, 0.5); + + observer.correct(u, + localY.plus(StateSpaceUtil.makeWhiteNoiseVector( + noiseStdDev))); + + var rdot = nextR.minus(r).div(dtSeconds); + u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1()))))); + + rdots.add(rdot); + + trajXs.add(ref.poseMeters.getTranslation().getX()); + trajYs.add(ref.poseMeters.getTranslation().getY()); + + observerXs.add(observer.getXhat().get(0, 0)); + observerYs.add(observer.getXhat().get(1, 0)); + + observerC.add(observer.getXhat(2)); + observerS.add(observer.getXhat(3)); + + observervl.add(observer.getXhat(4)); + observervr.add(observer.getXhat(5)); + + inputVl.add(u.get(0, 0)); + inputVr.add(u.get(1, 0)); + + timeData.add(i * dtSeconds); + + r = nextR; + observer.predict(u, dtSeconds); + trueXhat = RungeKutta.rungeKutta(UnscentedKalmanFilterTest::getDynamics, + trueXhat, u, dtSeconds); + } + + var localY = getLocalMeasurementModel(trueXhat, u); + observer.correct(u, localY); + + var globalY = getGlobalMeasurementModel(trueXhat, u); + var R = StateSpaceUtil.makeCostMatrix( + VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5)); + observer.correct(Nat.N6(), u, globalY, + UnscentedKalmanFilterTest::getGlobalMeasurementModel, R); + + final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds()); + + // var chartBuilder = new XYChartBuilder(); + // chartBuilder.title = "The Magic of Sensor Fusion, now with a " + // + observer.getClass().getSimpleName(); + // var xyPosChart = chartBuilder.build(); + + // xyPosChart.setXAxisTitle("X pos, meters"); + // xyPosChart.setYAxisTitle("Y pos, meters"); + // xyPosChart.addSeries("Trajectory", trajXs, trajYs); + // xyPosChart.addSeries("xHat", observerXs, observerYs); + + // var stateChart = new XYChartBuilder() + // .title("States (x-hat)").build(); + // stateChart.addSeries("Cos", timeData, observerC); + // stateChart.addSeries("Sin", timeData, observerS); + // stateChart.addSeries("vl, m/s", timeData, observervl); + // stateChart.addSeries("vr, m/s", timeData, observervr); + + // var inputChart = new XYChartBuilder().title("Inputs").build(); + // inputChart.addSeries("Left voltage", timeData, inputVl); + // inputChart.addSeries("Right voltage", timeData, inputVr); + + // var rdotChart = new XYChartBuilder().title("Rdot").build(); + // rdotChart.addSeries("xdot, or vx", timeData, rdots.stream().map(it -> it.get(0, 0)) + // .collect(Collectors.toList())); + // rdotChart.addSeries("ydot, or vy", timeData, rdots.stream().map(it -> it.get(1, 0)) + // .collect(Collectors.toList())); + // rdotChart.addSeries("cos dot", timeData, rdots.stream().map(it -> it.get(2, 0)) + // .collect(Collectors.toList())); + // rdotChart.addSeries("sin dot", timeData, rdots.stream().map(it -> it.get(3, 0)) + // .collect(Collectors.toList())); + // rdotChart.addSeries("vl dot, or al", timeData, rdots.stream().map(it -> it.get(4, 0)) + // .collect(Collectors.toList())); + // rdotChart.addSeries("vr dot, or ar", timeData, rdots.stream().map(it -> it.get(5, 0)) + // .collect(Collectors.toList())); + + // List charts = new ArrayList<>(); + // charts.add(xyPosChart); + // charts.add(stateChart); + // charts.add(inputChart); + // charts.add(rdotChart); + // new SwingWrapper<>(charts).displayChartMatrix(); + // try { + // Thread.sleep(1000000000); + // } catch (InterruptedException ex) { + // ex.printStackTrace(); + // } + + assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.25); + assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.25); + assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0); + assertEquals(0.0, observer.getXhat(3), 1.0); + assertEquals(0.0, observer.getXhat(4), 1.0); + } + + @Test + @SuppressWarnings({"LocalVariableName", "ParameterName", "PMD.AvoidInstantiatingObjectsInLoops"}) + public void testLinearUKF() { + var dt = 0.020; + var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006); + var observer = new UnscentedKalmanFilter<>(Nat.N1(), Nat.N1(), + (x, u) -> plant.getA().times(x).plus(plant.getB().times(u)), + plant::calculateY, + VecBuilder.fill(0.05), + VecBuilder.fill(1.0), + dt); + + var time = new ArrayList(); + var refData = new ArrayList(); + var xhat = new ArrayList(); + var udata = new ArrayList(); + var xdotData = new ArrayList(); + + var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt); + var discA = discABPair.getFirst(); + var discB = discABPair.getSecond(); + + Matrix ref = VecBuilder.fill(100); + Matrix u = VecBuilder.fill(0); + + Matrix xdot; + for (int i = 0; i < (2.0 / dt); i++) { + observer.predict(u, dt); + + u = discB.solve(ref.minus(discA.times(ref))); + + xdot = plant.getA().times(observer.getXhat()).plus(plant.getB().times(u)); + + time.add(i * dt); + refData.add(ref.get(0, 0)); + xhat.add(observer.getXhat(0)); + udata.add(u.get(0, 0)); + xdotData.add(xdot.get(0, 0)); + } + + // var chartBuilder = new XYChartBuilder(); + // chartBuilder.title = "The Magic of Sensor Fusion"; + // var chart = chartBuilder.build(); + + // chart.addSeries("Ref", time, refData); + // chart.addSeries("xHat", time, xhat); + // chart.addSeries("input", time, udata); + //// chart.addSeries("xdot", time, xdotData); + + // new SwingWrapper<>(chart).displayChart(); + // try { + // Thread.sleep(1000000000); + // } catch (InterruptedException e) { + // } + + assertEquals(ref.get(0, 0), observer.getXhat(0), 5); + } + + @Test + public void testUnscentedTransform() { + // From FilterPy + var ret = UnscentedKalmanFilter.unscentedTransform(Nat.N4(), Nat.N4(), + Matrix.mat(Nat.N4(), Nat.N9()).fill( + -0.9, -0.822540333075852, -0.8922540333075852, -0.9, + -0.9, -0.9774596669241481, -0.9077459666924148, -0.9, -0.9, + 1.0, 1.0, 1.077459666924148, 1.0, 1.0, 1.0, 0.9225403330758519, 1.0, 1.0, + -0.9, -0.9, -0.9, -0.822540333075852, -0.8922540333075852, -0.9, + -0.9, -0.9774596669241481, -0.9077459666924148, + 1.0, 1.0, 1.0, 1.0, 1.077459666924148, 1.0, 1.0, 1.0, 0.9225403330758519 + ), + VecBuilder.fill( + -132.33333333, + 16.66666667, + 16.66666667, + 16.66666667, + 16.66666667, + 16.66666667, + 16.66666667, + 16.66666667, + 16.66666667 + ), + VecBuilder.fill( + -129.34333333, + 16.66666667, + 16.66666667, + 16.66666667, + 16.66666667, + 16.66666667, + 16.66666667, + 16.66666667, + 16.66666667 + ) + ); + + assertTrue( + VecBuilder.fill(-0.9, 1, -0.9, 1).isEqual( + ret.getFirst(), 1E-5 + )); + + assertTrue( + Matrix.mat(Nat.N4(), Nat.N4()).fill( + 2.02000002e-01, 2.00000500e-02, -2.69044710e-29, + -4.59511477e-29, + 2.00000500e-02, 2.00001000e-01, -2.98781068e-29, + -5.12759588e-29, + -2.73372625e-29, -3.09882635e-29, 2.02000002e-01, + 2.00000500e-02, + -4.67065917e-29, -5.10705197e-29, 2.00000500e-02, + 2.00001000e-01 + ).isEqual( + ret.getSecond(), 1E-5 + )); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/math/StateSpaceUtilTest.java new file mode 100644 index 0000000000..252bf6e1b2 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/math/StateSpaceUtilTest.java @@ -0,0 +1,200 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.math; + +import java.util.ArrayList; +import java.util.List; + +import org.ejml.dense.row.MatrixFeatures_DDRM; +import org.ejml.simple.SimpleMatrix; +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Rotation2d; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.SimpleMatrixUtils; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; +import edu.wpi.first.wpiutil.math.numbers.N2; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertTrue; + +@SuppressWarnings("PMD.TooManyMethods") +public class StateSpaceUtilTest { + @Test + public void testCostArray() { + var mat = StateSpaceUtil.makeCostMatrix( + VecBuilder.fill(1.0, 2.0, 3.0)); + + assertEquals(1.0, mat.get(0, 0), 1e-3); + assertEquals(0.0, mat.get(0, 1), 1e-3); + assertEquals(0.0, mat.get(0, 2), 1e-3); + assertEquals(0.0, mat.get(1, 0), 1e-3); + assertEquals(1.0 / 4.0, mat.get(1, 1), 1e-3); + assertEquals(0.0, mat.get(1, 2), 1e-3); + assertEquals(0.0, mat.get(0, 2), 1e-3); + assertEquals(0.0, mat.get(1, 2), 1e-3); + assertEquals(1.0 / 9.0, mat.get(2, 2), 1e-3); + } + + @Test + public void testCovArray() { + var mat = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), + VecBuilder.fill(1.0, 2.0, 3.0)); + + assertEquals(1.0, mat.get(0, 0), 1e-3); + assertEquals(0.0, mat.get(0, 1), 1e-3); + assertEquals(0.0, mat.get(0, 2), 1e-3); + assertEquals(0.0, mat.get(1, 0), 1e-3); + assertEquals(4.0, mat.get(1, 1), 1e-3); + assertEquals(0.0, mat.get(1, 2), 1e-3); + assertEquals(0.0, mat.get(0, 2), 1e-3); + assertEquals(0.0, mat.get(1, 2), 1e-3); + assertEquals(9.0, mat.get(2, 2), 1e-3); + } + + @Test + @SuppressWarnings("LocalVariableName") + public void testIsStabilizable() { + Matrix A; + Matrix B = VecBuilder.fill(0, 1); + + // First eigenvalue is uncontrollable and unstable. + // Second eigenvalue is controllable and stable. + A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5); + assertFalse(StateSpaceUtil.isStabilizable(A, B)); + + // First eigenvalue is uncontrollable and marginally stable. + // Second eigenvalue is controllable and stable. + A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5); + assertFalse(StateSpaceUtil.isStabilizable(A, B)); + + // First eigenvalue is uncontrollable and stable. + // Second eigenvalue is controllable and stable. + A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5); + assertTrue(StateSpaceUtil.isStabilizable(A, B)); + + // First eigenvalue is uncontrollable and stable. + // Second eigenvalue is controllable and unstable. + A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2); + assertTrue(StateSpaceUtil.isStabilizable(A, B)); + } + + @Test + public void testMakeWhiteNoiseVector() { + var firstData = new ArrayList(); + var secondData = new ArrayList(); + for (int i = 0; i < 1000; i++) { + var noiseVec = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(1.0, 2.0)); + firstData.add(noiseVec.get(0, 0)); + secondData.add(noiseVec.get(1, 0)); + } + assertEquals(1.0, calculateStandardDeviation(firstData), 0.2); + assertEquals(2.0, calculateStandardDeviation(secondData), 0.2); + } + + private double calculateStandardDeviation(List numArray) { + double sum = 0.0; + double standardDeviation = 0.0; + int length = numArray.size(); + + for (double num : numArray) { + sum += num; + } + + double mean = sum / length; + + for (double num : numArray) { + standardDeviation += Math.pow(num - mean, 2); + } + + return Math.sqrt(standardDeviation / length); + } + + @Test + public void testDiscretizeA() { + var contA = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0); + var x0 = VecBuilder.fill(1, 1); + var discA = Discretization.discretizeA(contA, 1.0); + var x1Discrete = discA.times(x0); + + // We now have pos = vel = 1 and accel = 0, which should give us: + var x1Truth = VecBuilder.fill(x0.get(0, 0) + 1.0 * x0.get(1, 0), + x0.get(1, 0)); + assertTrue(x1Truth.isEqual(x1Discrete, 1E-4)); + } + + @SuppressWarnings("LocalVariableName") + @Test + public void testDiscretizeAB() { + var contA = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0); + var contB = VecBuilder.fill(0, 1); + var x0 = VecBuilder.fill(1, 1); + var u = VecBuilder.fill(1); + + var abPair = Discretization.discretizeAB(contA, contB, 1.0); + + var x1Discrete = abPair.getFirst().times(x0).plus(abPair.getSecond().times(u)); + + // We now have pos = vel = accel = 1, which should give us: + var x1Truth = VecBuilder.fill(x0.get(0, 0) + x0.get(1, 0) + 0.5 * u.get(0, 0), x0.get(0, 0) + + u.get(0, 0)); + + assertTrue(x1Truth.isEqual(x1Discrete, 1E-4)); + } + + @Test + public void testMatrixExp() { + Matrix wrappedMatrix = Matrix.eye(Nat.N2()); + var wrappedResult = wrappedMatrix.exp(); + + assertTrue(wrappedResult.isEqual( + Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9)); + + var matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4); + wrappedResult = matrix.times(0.01).exp(); + + assertTrue(wrappedResult.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, + 0.03076368, 1.04111993), 1E-8)); + } + + @Test + public void testSimpleMatrixExp() { + SimpleMatrix matrix = SimpleMatrixUtils.eye(2); + var result = SimpleMatrixUtils.exp(matrix); + + assertTrue(MatrixFeatures_DDRM.isIdentical( + result.getDDRM(), + new SimpleMatrix(2, 2, true, new double[]{Math.E, 0, 0, Math.E}).getDDRM(), + 1E-9 + )); + + matrix = new SimpleMatrix(2, 2, true, new double[]{1, 2, 3, 4}); + result = SimpleMatrixUtils.exp(matrix.scale(0.01)); + + assertTrue(MatrixFeatures_DDRM.isIdentical( + result.getDDRM(), + new SimpleMatrix(2, 2, true, new double[]{1.01035625, 0.02050912, + 0.03076368, 1.04111993}).getDDRM(), + 1E-8 + )); + } + + @Test + public void testPoseToVector() { + Pose2d pose = new Pose2d(1, 2, new Rotation2d(3)); + var vector = StateSpaceUtil.poseToVector(pose); + assertEquals(pose.getTranslation().getX(), vector.get(0, 0), 1e-6); + assertEquals(pose.getTranslation().getY(), vector.get(1, 0), 1e-6); + assertEquals(pose.getRotation().getRadians(), vector.get(2, 0), 1e-6); + } + +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/system/LinearSystemIDTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/system/LinearSystemIDTest.java new file mode 100644 index 0000000000..8af7ef02e2 --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/system/LinearSystemIDTest.java @@ -0,0 +1,91 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.system; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpilibj.system.plant.DCMotor; +import edu.wpi.first.wpilibj.system.plant.LinearSystemId; +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertTrue; + +class LinearSystemIDTest { + @Test + public void testDrivetrainVelocitySystem() { + var model = LinearSystemId.createDrivetrainVelocitySystem( + DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6 + ); + assertTrue(model.getA().isEqual(Matrix.mat(Nat.N2(), + Nat.N2()).fill(-10.14132, 3.06598, 3.06598, -10.14132), 0.001)); + + assertTrue(model.getB().isEqual(Matrix.mat(Nat.N2(), + Nat.N2()).fill(4.2590, -1.28762, -1.2876, 4.2590), 0.001)); + + assertTrue(model.getC().isEqual(Matrix.mat(Nat.N2(), + Nat.N2()).fill(1.0, 0.0, 0.0, 1.0), 0.001)); + + assertTrue(model.getD().isEqual(Matrix.mat(Nat.N2(), + Nat.N2()).fill(0.0, 0.0, 0.0, 0.0), 0.001)); + } + + @Test + public void testElevatorSystem() { + + var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12); + assertTrue(model.getA().isEqual(Matrix.mat(Nat.N2(), + Nat.N2()).fill(0, 1, 0, -99.05473), 0.001)); + + assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001)); + + assertTrue(model.getC().isEqual(Matrix.mat(Nat.N1(), + Nat.N2()).fill(1, 0), 0.001)); + + assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001)); + } + + @Test + public void testFlywheelSystem() { + var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0); + assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001)); + + assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001)); + + assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001)); + + assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001)); + } + + @Test + public void testIdentifyPositionSystem() { + // By controls engineering in frc, + // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u + var kv = 1.0; + var ka = 0.5; + var model = LinearSystemId.identifyPositionSystem(kv, ka); + + assertEquals(model.getA(), Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka)); + assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka)); + } + + @Test + public void testIdentifyVelocitySystem() { + // By controls engineering in frc, + // V = kv * velocity + ka * acceleration + // x-dot = -kv/ka * v + 1/ka \cdot V + var kv = 1.0; + var ka = 0.5; + var model = LinearSystemId.identifyVelocitySystem(kv, ka); + + assertEquals(model.getA(), VecBuilder.fill(-kv / ka)); + assertEquals(model.getB(), VecBuilder.fill(1 / ka)); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java b/wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java new file mode 100644 index 0000000000..de392955bf --- /dev/null +++ b/wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package edu.wpi.first.wpilibj.system; + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.wpiutil.math.Matrix; +import edu.wpi.first.wpiutil.math.Nat; +import edu.wpi.first.wpiutil.math.VecBuilder; +import edu.wpi.first.wpiutil.math.numbers.N1; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +public class RungeKuttaTest { + @Test + @SuppressWarnings({"ParameterName", "LocalVariableName"}) + public void testExponential() { + + Matrix y0 = VecBuilder.fill(0.0); + + //noinspection SuspiciousNameCombination + var y1 = RungeKutta.rungeKutta((Matrix x) -> { + var y = new Matrix<>(Nat.N1(), Nat.N1()); + y.set(0, 0, Math.exp(x.get(0, 0))); + return y; }, + y0, 0.1 + ); + + assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3); + } +} diff --git a/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java index 8ec462f2df..1af0625c21 100644 --- a/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java +++ b/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java @@ -8,8 +8,6 @@ package edu.wpi.first.wpiutil.math; import org.ejml.data.SingularMatrixException; -import org.ejml.dense.row.MatrixFeatures_DDRM; -import org.ejml.simple.SimpleMatrix; import org.junit.jupiter.api.Test; import edu.wpi.first.wpiutil.math.numbers.N1; @@ -17,107 +15,106 @@ import edu.wpi.first.wpiutil.math.numbers.N2; import edu.wpi.first.wpiutil.math.numbers.N3; import edu.wpi.first.wpiutil.math.numbers.N4; +import static org.junit.jupiter.api.Assertions.assertEquals; import static org.junit.jupiter.api.Assertions.assertThrows; import static org.junit.jupiter.api.Assertions.assertTrue; public class MatrixTest { @Test void testMatrixMultiplication() { - var mat1 = MatrixUtils.mat(Nat.N2(), Nat.N2()) + var mat1 = Matrix.mat(Nat.N2(), Nat.N2()) .fill(2.0, 1.0, 0.0, 1.0); - var mat2 = MatrixUtils.mat(Nat.N2(), Nat.N2()) + var mat2 = Matrix.mat(Nat.N2(), Nat.N2()) .fill(3.0, 0.0, 0.0, 2.5); Matrix result = mat1.times(mat2); - assertTrue(MatrixFeatures_DDRM.isEquals( - MatrixUtils.mat(Nat.N2(), Nat.N2()) - .fill(6.0, 2.5, - 0.0, 2.5).getStorage().getDDRM(), - result.getStorage().getDDRM() - )); + assertEquals(result, Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 2.5, 0.0, 2.5)); - var mat3 = MatrixUtils.mat(Nat.N2(), Nat.N3()) + var mat3 = Matrix.mat(Nat.N2(), Nat.N3()) .fill(1.0, 3.0, 0.5, 2.0, 4.3, 1.2); - var mat4 = MatrixUtils.mat(Nat.N3(), Nat.N4()) + var mat4 = Matrix.mat(Nat.N3(), Nat.N4()) .fill(3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0); Matrix result2 = mat3.times(mat4); - assertTrue(MatrixFeatures_DDRM.isIdentical( - MatrixUtils.mat(Nat.N2(), Nat.N4()) + assertTrue(Matrix.mat(Nat.N2(), Nat.N4()) .fill(12.5, 5.55, 7.8, 14.3, - 22.13, 9.82, 13.28, 23.53).getStorage().getDDRM(), - result2.getStorage().getDDRM(), - 1E-9 + 22.13, 9.82, 13.28, 23.53).isEqual( + result2, + 1E-9 )); } @Test void testMatrixVectorMultiplication() { - var mat = MatrixUtils.mat(Nat.N2(), Nat.N2()) + var mat = Matrix.mat(Nat.N2(), Nat.N2()) .fill(1.0, 1.0, 0.0, 1.0); - var vec = MatrixUtils.vec(Nat.N2()) - .fill(3.0, - 2.0); + var vec = VecBuilder.fill(3.0, 2.0); Matrix result = mat.times(vec); - assertTrue(MatrixFeatures_DDRM.isEquals( - MatrixUtils.vec(Nat.N2()) - .fill(5.0, - 2.0).getStorage().getDDRM(), - result.getStorage().getDDRM() - )); + assertEquals(VecBuilder.fill(5.0, 2.0), result); } @Test void testTranspose() { - Matrix vec = MatrixUtils.vec(Nat.N3()) + Matrix vec = VecBuilder .fill(1.0, 2.0, 3.0); Matrix transpose = vec.transpose(); - assertTrue(MatrixFeatures_DDRM.isEquals( - MatrixUtils.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0).getStorage() - .getDDRM(), - transpose.getStorage().getDDRM() - )); + assertEquals(Matrix.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0), transpose); + } + + @Test + void testSolve() { + var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0); + var vec1 = VecBuilder.fill(1.0, 2.0); + + var solve1 = mat1.solve(vec1); + + assertEquals(VecBuilder.fill(0.0, 0.5), solve1); + + var mat2 = Matrix.mat(Nat.N3(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + var vec2 = VecBuilder.fill(1.0, 2.0, 3.0); + + var solve2 = mat2.solve(vec2); + + assertEquals(VecBuilder.fill(0.0, 0.5), solve2); } @Test void testInverse() { - var mat = MatrixUtils.mat(Nat.N3(), Nat.N3()) + var mat = Matrix.mat(Nat.N3(), Nat.N3()) .fill(1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5); var inv = mat.inv(); - assertTrue(MatrixFeatures_DDRM.isIdentical( - MatrixUtils.eye(Nat.N3()).getStorage().getDDRM(), - mat.times(inv).getStorage().getDDRM(), + assertTrue(Matrix.eye(Nat.N3()).isEqual( + mat.times(inv), 1E-9 )); - assertTrue(MatrixFeatures_DDRM.isIdentical( - MatrixUtils.eye(Nat.N3()).getStorage().getDDRM(), - inv.times(mat).getStorage().getDDRM(), + assertTrue(Matrix.eye(Nat.N3()).isEqual( + inv.times(mat), 1E-9 )); } @Test void testUninvertableMatrix() { - var singularMatrix = MatrixUtils.mat(Nat.N2(), Nat.N2()) + var singularMatrix = Matrix.mat(Nat.N2(), Nat.N2()) .fill(2.0, 1.0, 2.0, 1.0); @@ -126,85 +123,52 @@ public class MatrixTest { @Test void testMatrixScalarArithmetic() { - var mat = MatrixUtils.mat(Nat.N2(), Nat.N2()) + var mat = Matrix.mat(Nat.N2(), Nat.N2()) .fill(1.0, 2.0, 3.0, 4.0); + assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 4.0, 5.0, 6.0), mat.plus(2.0)); - assertTrue(MatrixFeatures_DDRM.isEquals( - MatrixUtils.mat(Nat.N2(), Nat.N2()) - .fill(3.0, 4.0, - 5.0, 6.0).getStorage().getDDRM(), - mat.plus(2.0).getStorage().getDDRM() - )); + assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 2.0, 3.0), mat.minus(1.0)); - assertTrue(MatrixFeatures_DDRM.isEquals( - MatrixUtils.mat(Nat.N2(), Nat.N2()) - .fill(0.0, 1.0, - 2.0, 3.0).getStorage().getDDRM(), - mat.minus(1.0).getStorage().getDDRM() - )); + assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 4.0, 6.0, 8.0), mat.times(2.0)); - assertTrue(MatrixFeatures_DDRM.isEquals( - MatrixUtils.mat(Nat.N2(), Nat.N2()) - .fill(2.0, 4.0, - 6.0, 8.0).getStorage().getDDRM(), - mat.times(2.0).getStorage().getDDRM() - )); - - assertTrue(MatrixFeatures_DDRM.isIdentical( - MatrixUtils.mat(Nat.N2(), Nat.N2()) - .fill(0.5, 1.0, - 1.5, 2.0).getStorage().getDDRM(), - mat.div(2.0).getStorage().getDDRM(), + assertTrue(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.5, 1.0, 1.5, 2.0).isEqual( + mat.div(2.0), 1E-3 )); } @Test void testMatrixMatrixArithmetic() { - var mat1 = MatrixUtils.mat(Nat.N2(), Nat.N2()) + var mat1 = Matrix.mat(Nat.N2(), Nat.N2()) .fill(1.0, 2.0, 3.0, 4.0); - var mat2 = MatrixUtils.mat(Nat.N2(), Nat.N2()) + var mat2 = Matrix.mat(Nat.N2(), Nat.N2()) .fill(5.0, 6.0, 7.0, 8.0); - assertTrue(MatrixFeatures_DDRM.isEquals( - MatrixUtils.mat(Nat.N2(), Nat.N2()) - .fill(-4.0, -4.0, - -4.0, -4.0).getStorage().getDDRM(), - mat1.minus(mat2).getStorage().getDDRM() - )); + assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(-4.0, -4.0, -4.0, -4.0), + mat1.minus(mat2) + ); - assertTrue(MatrixFeatures_DDRM.isEquals( - MatrixUtils.mat(Nat.N2(), Nat.N2()) - .fill(6.0, 8.0, - 10.0, 12.0).getStorage().getDDRM(), - mat1.plus(mat2).getStorage().getDDRM() - )); + assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 8.0, 10.0, 12.0), + mat1.plus(mat2) + ); } @Test void testMatrixExponential() { - SimpleMatrix matrix = MatrixUtils.eye(Nat.N2()).getStorage(); - var result = SimpleMatrixUtils.expm(matrix); + var matrix = Matrix.eye(Nat.N2()); + var result = matrix.exp(); - assertTrue(MatrixFeatures_DDRM.isIdentical( - result.getDDRM(), - new SimpleMatrix(2, 2, true, new double[]{Math.E, 0, 0, Math.E}).getDDRM(), - 1E-9 - )); + assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9)); - matrix = new SimpleMatrix(2, 2, true, new double[]{1, 2, 3, 4}); - result = SimpleMatrixUtils.expm(matrix.scale(0.01)); + matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4); + result = matrix.times(0.01).exp(); - assertTrue(MatrixFeatures_DDRM.isIdentical( - result.getDDRM(), - new SimpleMatrix(2, 2, true, new double[]{1.01035625, 0.02050912, - 0.03076368, 1.04111993}).getDDRM(), - 1E-8 - )); + assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, + 0.03076368, 1.04111993), 1E-8)); } } diff --git a/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/wpimath/src/test/native/cpp/StateSpaceTest.cpp new file mode 100644 index 0000000000..fa16c18cca --- /dev/null +++ b/wpimath/src/test/native/cpp/StateSpaceTest.cpp @@ -0,0 +1,77 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include +#include + +#include "Eigen/Core" +#include "frc/controller/LinearPlantInversionFeedforward.h" +#include "frc/controller/LinearQuadraticRegulator.h" +#include "frc/estimator/KalmanFilter.h" +#include "frc/system/LinearSystem.h" +#include "frc/system/LinearSystemLoop.h" +#include "frc/system/plant/DCMotor.h" +#include "frc/system/plant/LinearSystemId.h" +#include "units/time.h" + +namespace frc { + +constexpr double kPositionStddev = 0.0001; +constexpr auto kDt = 0.00505_s; + +class StateSpace : public testing::Test { + public: + LinearSystem<2, 1, 1> plant = [] { + auto motors = DCMotor::Vex775Pro(2); + + // Carriage mass + constexpr auto m = 5_kg; + + // Radius of pulley + constexpr auto r = 0.0181864_m; + + // Gear ratio + constexpr double G = 40.0 / 40.0; + + return frc::LinearSystemId::ElevatorSystem(motors, m, r, G); + }(); + LinearQuadraticRegulator<2, 1> controller{plant, {0.02, 0.4}, {12.0}, kDt}; + KalmanFilter<2, 1, 1> observer{plant, {0.05, 1.0}, {0.0001}, kDt}; + LinearPlantInversionFeedforward<2, 1> feedforward{plant, kDt}; + LinearSystemLoop<2, 1, 1> loop{plant, controller, feedforward, observer, + 12_V}; +}; + +void Update(LinearSystemLoop<2, 1, 1>& loop, double noise) { + Eigen::Matrix y = + loop.Plant().CalculateY(loop.Xhat(), loop.U()) + + Eigen::Matrix(noise); + loop.Correct(y); + loop.Predict(kDt); +} + +TEST_F(StateSpace, CorrectPredictLoop) { + std::default_random_engine generator; + std::normal_distribution dist{0.0, kPositionStddev}; + + Eigen::Matrix references; + references << 2.0, 0.0; + loop.SetNextR(references); + + for (int i = 0; i < 1000; i++) { + Update(loop, dist(generator)); + EXPECT_PRED_FORMAT2(testing::DoubleLE, -12.0, loop.U(0)); + EXPECT_PRED_FORMAT2(testing::DoubleLE, loop.U(0), 12.0); + } + + EXPECT_NEAR(loop.Xhat(0), 2.0, 0.05); + EXPECT_NEAR(loop.Xhat(1), 0.0, 0.5); +} + +} // namespace frc diff --git a/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp new file mode 100644 index 0000000000..24e9cf296b --- /dev/null +++ b/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp @@ -0,0 +1,148 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include + +#include "Eigen/Core" +#include "frc/StateSpaceUtil.h" +#include "frc/system/RungeKutta.h" + +TEST(StateSpaceUtilTest, MakeMatrix) { + // Column vector + Eigen::Matrix mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0); + EXPECT_NEAR(mat1(0), 1.0, 1e-3); + EXPECT_NEAR(mat1(1), 2.0, 1e-3); + + // Row vector + Eigen::Matrix mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0); + EXPECT_NEAR(mat2(0), 1.0, 1e-3); + EXPECT_NEAR(mat2(1), 2.0, 1e-3); + + // Square matrix + Eigen::Matrix mat3 = frc::MakeMatrix<2, 2>(1.0, 2.0, 3.0, 4.0); + EXPECT_NEAR(mat3(0, 0), 1.0, 1e-3); + EXPECT_NEAR(mat3(0, 1), 2.0, 1e-3); + EXPECT_NEAR(mat3(1, 0), 3.0, 1e-3); + EXPECT_NEAR(mat3(1, 1), 4.0, 1e-3); + + // Nonsquare matrix with more rows than columns + Eigen::Matrix mat4 = + frc::MakeMatrix<3, 2>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + EXPECT_NEAR(mat4(0, 0), 1.0, 1e-3); + EXPECT_NEAR(mat4(0, 1), 2.0, 1e-3); + EXPECT_NEAR(mat4(1, 0), 3.0, 1e-3); + EXPECT_NEAR(mat4(1, 1), 4.0, 1e-3); + EXPECT_NEAR(mat4(2, 0), 5.0, 1e-3); + EXPECT_NEAR(mat4(2, 1), 6.0, 1e-3); + + // Nonsquare matrix with more columns than rows + Eigen::Matrix mat5 = + frc::MakeMatrix<2, 3>(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + EXPECT_NEAR(mat5(0, 0), 1.0, 1e-3); + EXPECT_NEAR(mat5(0, 1), 2.0, 1e-3); + EXPECT_NEAR(mat5(0, 2), 3.0, 1e-3); + EXPECT_NEAR(mat5(1, 0), 4.0, 1e-3); + EXPECT_NEAR(mat5(1, 1), 5.0, 1e-3); + EXPECT_NEAR(mat5(1, 2), 6.0, 1e-3); +} + +TEST(StateSpaceUtilTest, CostParameterPack) { + Eigen::Matrix mat = frc::MakeCostMatrix(1.0, 2.0, 3.0); + EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); + EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 0), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 1), 1.0 / 4.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3); +} + +TEST(StateSpaceUtilTest, CostArray) { + Eigen::Matrix mat = frc::MakeCostMatrix<3>({1.0, 2.0, 3.0}); + EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); + EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 0), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 1), 1.0 / 4.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(2, 2), 1.0 / 9.0, 1e-3); +} + +TEST(StateSpaceUtilTest, CovParameterPack) { + Eigen::Matrix mat = frc::MakeCovMatrix(1.0, 2.0, 3.0); + EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); + EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 0), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 1), 4.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(2, 2), 9.0, 1e-3); +} + +TEST(StateSpaceUtilTest, CovArray) { + Eigen::Matrix mat = frc::MakeCovMatrix<3>({1.0, 2.0, 3.0}); + EXPECT_NEAR(mat(0, 0), 1.0, 1e-3); + EXPECT_NEAR(mat(0, 1), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 0), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 1), 4.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(0, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(1, 2), 0.0, 1e-3); + EXPECT_NEAR(mat(2, 2), 9.0, 1e-3); +} + +TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) { + Eigen::Matrix vec = frc::MakeWhiteNoiseVector(2.0, 3.0); + static_cast(vec); +} + +TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) { + Eigen::Matrix vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0}); + static_cast(vec); +} + +TEST(StateSpaceUtilTest, IsStabilizable) { + Eigen::Matrix A; + Eigen::Matrix B; + B << 0, 1; + + // We separate the result of IsStabilizable from the assertion because + // templates break gtest. + + // First eigenvalue is uncontrollable and unstable. + // Second eigenvalue is controllable and stable. + A << 1.2, 0, 0, 0.5; + bool ret = frc::IsStabilizable<2, 1>(A, B); + EXPECT_FALSE(ret); + + // First eigenvalue is uncontrollable and marginally stable. + // Second eigenvalue is controllable and stable. + A << 1, 0, 0, 0.5; + ret = frc::IsStabilizable<2, 1>(A, B); + EXPECT_FALSE(ret); + + // First eigenvalue is uncontrollable and stable. + // Second eigenvalue is controllable and stable. + A << 0.2, 0, 0, 0.5; + ret = frc::IsStabilizable<2, 1>(A, B); + EXPECT_TRUE(ret); + + // First eigenvalue is uncontrollable and stable. + // Second eigenvalue is controllable and unstable. + A << 0.2, 0, 0, 1.2; + ret = frc::IsStabilizable<2, 1>(A, B); + EXPECT_TRUE(ret); +} diff --git a/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp new file mode 100644 index 0000000000..79f1a6d017 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp @@ -0,0 +1,72 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include + +#include "Eigen/Core" +#include "frc/StateSpaceUtil.h" +#include "frc/controller/ControlAffinePlantInversionFeedforward.h" +#include "units/time.h" + +namespace frc { + +Eigen::Matrix Dynamics(const Eigen::Matrix& x, + const Eigen::Matrix& u) { + Eigen::Matrix result; + + result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x) + + (frc::MakeMatrix<2, 1>(0.0, 1.0) * u); + + return result; +} + +Eigen::Matrix StateDynamics( + const Eigen::Matrix& x) { + Eigen::Matrix result; + + result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x); + + return result; +} + +TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) { + std::function(const Eigen::Matrix&, + const Eigen::Matrix&)> + modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); }; + + frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{ + modelDynamics, units::second_t(0.02)}; + + Eigen::Matrix r; + r << 2, 2; + Eigen::Matrix nextR; + nextR << 3, 3; + + EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6); +} + +TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) { + std::function(const Eigen::Matrix&)> + modelDynamics = [](auto& x) { return StateDynamics(x); }; + + Eigen::Matrix B; + B << 0, 1; + + frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{ + modelDynamics, B, units::second_t(0.02)}; + + Eigen::Matrix r; + r << 2, 2; + Eigen::Matrix nextR; + nextR << 3, 3; + + EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6); +} + +} // namespace frc diff --git a/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp new file mode 100644 index 0000000000..abc22d9b52 --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include + +#include "Eigen/Core" +#include "frc/controller/LinearPlantInversionFeedforward.h" +#include "units/time.h" + +namespace frc { + +TEST(LinearPlantInversionFeedforwardTest, Calculate) { + Eigen::Matrix A; + A << 1, 0, 0, 1; + + Eigen::Matrix B; + B << 0, 1; + + frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B, + units::second_t(0.02)}; + + Eigen::Matrix r; + r << 2, 2; + Eigen::Matrix nextR; + nextR << 3, 3; + + EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002); +} + +} // namespace frc diff --git a/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp new file mode 100644 index 0000000000..eecaf34c2c --- /dev/null +++ b/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp @@ -0,0 +1,88 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include + +#include "Eigen/Core" +#include "frc/controller/LinearQuadraticRegulator.h" +#include "frc/system/LinearSystem.h" +#include "frc/system/plant/DCMotor.h" +#include "frc/system/plant/LinearSystemId.h" +#include "units/time.h" + +namespace frc { + +TEST(LinearQuadraticRegulatorTest, ElevatorGains) { + LinearSystem<2, 1, 1> plant = [] { + auto motors = DCMotor::Vex775Pro(2); + + // Carriage mass + constexpr auto m = 5_kg; + + // Radius of pulley + constexpr auto r = 0.0181864_m; + + // Gear ratio + constexpr double G = 40.0 / 40.0; + + return frc::LinearSystemId::ElevatorSystem(motors, m, r, G); + }(); + LinearQuadraticRegulator<2, 1> controller{ + plant, {0.02, 0.4}, {12.0}, 0.00505_s}; + + EXPECT_NEAR(522.15314269, controller.K(0, 0), 1e-6); + EXPECT_NEAR(38.20138596, controller.K(0, 1), 1e-6); +} + +TEST(LinearQuadraticRegulatorTest, ArmGains) { + LinearSystem<2, 1, 1> plant = [] { + auto motors = DCMotor::Vex775Pro(2); + + // Carriage mass + constexpr auto m = 4_kg; + + // Radius of pulley + constexpr auto r = 0.4_m; + + // Gear ratio + constexpr double G = 100.0; + + return frc::LinearSystemId::SingleJointedArmSystem( + motors, 1.0 / 3.0 * m * r * r, G); + }(); + + LinearQuadraticRegulator<2, 1> controller{ + plant, {0.01745, 0.08726}, {12.0}, 0.00505_s}; + + EXPECT_NEAR(19.16, controller.K(0, 0), 1e-1); + EXPECT_NEAR(3.32, controller.K(0, 1), 1e-1); +} + +TEST(LinearQuadraticRegulatorTest, FourMotorElevator) { + LinearSystem<2, 1, 1> plant = [] { + auto motors = DCMotor::Vex775Pro(4); + + // Carriage mass + constexpr auto m = 8_kg; + + // Radius of pulley + constexpr auto r = 0.75_in; + + // Gear ratio + constexpr double G = 14.67; + + return frc::LinearSystemId::ElevatorSystem(motors, m, r, G); + }(); + LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.020_s}; + + EXPECT_NEAR(10.38, controller.K(0, 0), 1e-1); + EXPECT_NEAR(0.69, controller.K(0, 1), 1e-1); +} + +} // namespace frc diff --git a/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp new file mode 100644 index 0000000000..387593b407 --- /dev/null +++ b/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp @@ -0,0 +1,172 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include +#include + +#include "Eigen/Core" +#include "Eigen/QR" +#include "frc/StateSpaceUtil.h" +#include "frc/estimator/ExtendedKalmanFilter.h" +#include "frc/system/NumericalJacobian.h" +#include "frc/system/plant/DCMotor.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "units/moment_of_inertia.h" + +namespace { + +Eigen::Matrix Dynamics(const Eigen::Matrix& x, + const Eigen::Matrix& u) { + auto motors = frc::DCMotor::CIM(2); + + // constexpr double Glow = 15.32; // Low gear ratio + constexpr double Ghigh = 7.08; // High gear ratio + constexpr auto rb = 0.8382_m / 2.0; // Robot radius + constexpr auto r = 0.0746125_m; // Wheel radius + constexpr auto m = 63.503_kg; // Robot mass + constexpr auto J = 5.6_kg_sq_m; // Robot moment of inertia + + auto C1 = -std::pow(Ghigh, 2) * motors.Kt / + (motors.Kv * motors.R * units::math::pow<2>(r)); + auto C2 = Ghigh * motors.Kt / (motors.R * r); + auto k1 = (1 / m + units::math::pow<2>(rb) / J); + auto k2 = (1 / m - units::math::pow<2>(rb) / J); + + units::meters_per_second_t vl{x(3)}; + units::meters_per_second_t vr{x(4)}; + units::volt_t Vl{u(0)}; + units::volt_t Vr{u(1)}; + + Eigen::Matrix result; + auto v = 0.5 * (vl + vr); + result(0) = v.to() * std::cos(x(2)); + result(1) = v.to() * std::sin(x(2)); + result(2) = ((vr - vl) / (2.0 * rb)).to(); + result(3) = + k1.to() * ((C1 * vl).to() + (C2 * Vl).to()) + + k2.to() * ((C1 * vr).to() + (C2 * Vr).to()); + result(4) = + k2.to() * ((C1 * vl).to() + (C2 * Vl).to()) + + k1.to() * ((C1 * vr).to() + (C2 * Vr).to()); + return result; +} + +Eigen::Matrix LocalMeasurementModel( + const Eigen::Matrix& x, + const Eigen::Matrix& u) { + static_cast(u); + Eigen::Matrix y; + y << x(2), x(3), x(4); + return y; +} + +Eigen::Matrix GlobalMeasurementModel( + const Eigen::Matrix& x, + const Eigen::Matrix& u) { + static_cast(u); + Eigen::Matrix y; + y << x(0), x(1), x(2), x(3), x(4); + return y; +} +} // namespace + +TEST(ExtendedKalmanFilterTest, Init) { + constexpr auto dt = 0.00505_s; + + frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics, + LocalMeasurementModel, + {0.5, 0.5, 10.0, 1.0, 1.0}, + {0.0001, 0.01, 0.01}, + dt}; + Eigen::Matrix u; + u << 12.0, 12.0; + observer.Predict(u, dt); + + auto localY = LocalMeasurementModel(observer.Xhat(), u); + observer.Correct(u, localY); + + auto globalY = GlobalMeasurementModel(observer.Xhat(), u); + auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01); + observer.Correct<5>(u, globalY, GlobalMeasurementModel, R); +} + +TEST(ExtendedKalmanFilterTest, Convergence) { + constexpr auto dt = 0.00505_s; + constexpr auto rb = 0.8382_m / 2.0; // Robot radius + + frc::ExtendedKalmanFilter<5, 2, 3> observer{Dynamics, + LocalMeasurementModel, + {0.5, 0.5, 10.0, 1.0, 1.0}, + {0.0001, 0.5, 0.5}, + dt}; + + auto waypoints = + std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad}, + frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}}; + auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + waypoints, {8.8_mps, 0.1_mps_sq}); + + Eigen::Matrix r = Eigen::Matrix::Zero(); + + Eigen::Matrix nextR; + Eigen::Matrix u = Eigen::Matrix::Zero(); + + auto B = frc::NumericalJacobianU<5, 5, 2>( + Dynamics, Eigen::Matrix::Zero(), + Eigen::Matrix::Zero()); + + observer.SetXhat(frc::MakeMatrix<5, 1>( + trajectory.InitialPose().Translation().X().to(), + trajectory.InitialPose().Translation().Y().to(), + trajectory.InitialPose().Rotation().Radians().to(), 0.0, 0.0)); + + auto totalTime = trajectory.TotalTime(); + for (size_t i = 0; i < (totalTime / dt).to(); ++i) { + auto ref = trajectory.Sample(dt * i); + units::meters_per_second_t vl = + ref.velocity * (1 - (ref.curvature * rb).to()); + units::meters_per_second_t vr = + ref.velocity * (1 + (ref.curvature * rb).to()); + + nextR(0) = ref.pose.Translation().X().to(); + nextR(1) = ref.pose.Translation().Y().to(); + nextR(2) = ref.pose.Rotation().Radians().to(); + nextR(3) = vl.to(); + nextR(4) = vr.to(); + + auto localY = + LocalMeasurementModel(nextR, Eigen::Matrix::Zero()); + observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5)); + + Eigen::Matrix rdot = (nextR - r) / dt.to(); + u = B.householderQr().solve( + rdot - Dynamics(r, Eigen::Matrix::Zero())); + + observer.Predict(u, dt); + + r = nextR; + } + + auto localY = LocalMeasurementModel(observer.Xhat(), u); + observer.Correct(u, localY); + + auto globalY = GlobalMeasurementModel(observer.Xhat(), u); + auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5); + observer.Correct<5>(u, globalY, GlobalMeasurementModel, R); + + auto finalPosition = trajectory.Sample(trajectory.TotalTime()); + ASSERT_NEAR(finalPosition.pose.Translation().X().template to(), + observer.Xhat(0), 1.0); + ASSERT_NEAR(finalPosition.pose.Translation().Y().template to(), + observer.Xhat(1), 1.0); + ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to(), + observer.Xhat(2), 1.0); + ASSERT_NEAR(0.0, observer.Xhat(3), 1.0); + ASSERT_NEAR(0.0, observer.Xhat(4), 1.0); +} diff --git a/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp new file mode 100644 index 0000000000..53c54effb0 --- /dev/null +++ b/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp @@ -0,0 +1,24 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include +#include + +#include "Eigen/Core" +#include "frc/estimator/KalmanFilter.h" +#include "frc/system/plant/DCMotor.h" +#include "frc/system/plant/LinearSystemId.h" +#include "units/moment_of_inertia.h" +#include "units/time.h" + +TEST(KalmanFilterTest, Flywheel) { + auto motor = frc::DCMotor::NEO(); + auto flywheel = frc::LinearSystemId::FlywheelSystem(motor, 1_kg_sq_m, 1.0); + frc::KalmanFilter<1, 1, 1> kf{flywheel, {1}, {1}, 5_ms}; +} diff --git a/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp new file mode 100644 index 0000000000..ace5e79558 --- /dev/null +++ b/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp @@ -0,0 +1,41 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "frc/StateSpaceUtil.h" +#include "frc/estimator/MerweScaledSigmaPoints.h" + +namespace drake { +namespace math { +namespace { +TEST(MerweScaledSigmaPointsTest, TestZeroMean) { + frc::MerweScaledSigmaPoints<2> sigmaPoints; + auto points = + sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(0.0, 0.0), + frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0)); + + EXPECT_TRUE( + (points - frc::MakeMatrix<2, 5>(0.0, 0.00173205, 0.0, -0.00173205, 0.0, + 0.0, 0.0, 0.00173205, 0.0, -0.00173205)) + .norm() < 1e-3); +} + +TEST(MerweScaledSigmaPointsTest, TestNonzeroMean) { + frc::MerweScaledSigmaPoints<2> sigmaPoints; + auto points = + sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(1.0, 2.0), + frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 10.0)); + + EXPECT_TRUE( + (points - frc::MakeMatrix<2, 5>(1.0, 1.00173205, 1.0, 0.998268, 1.0, 2.0, + 2.0, 2.00548, 2.0, 1.99452)) + .norm() < 1e-3); +} +} // namespace +} // namespace math +} // namespace drake diff --git a/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp new file mode 100644 index 0000000000..0017b42cc9 --- /dev/null +++ b/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp @@ -0,0 +1,176 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include +#include + +#include "Eigen/Core" +#include "Eigen/QR" +#include "frc/StateSpaceUtil.h" +#include "frc/estimator/UnscentedKalmanFilter.h" +#include "frc/system/NumericalJacobian.h" +#include "frc/system/RungeKutta.h" +#include "frc/system/plant/DCMotor.h" +#include "frc/trajectory/TrajectoryGenerator.h" +#include "units/moment_of_inertia.h" + +namespace { + +Eigen::Matrix Dynamics(const Eigen::Matrix& x, + const Eigen::Matrix& u) { + auto motors = frc::DCMotor::CIM(2); + + // constexpr double Glow = 15.32; // Low gear ratio + constexpr double Ghigh = 7.08; // High gear ratio + constexpr auto rb = 0.8382_m / 2.0; // Robot radius + constexpr auto r = 0.0746125_m; // Wheel radius + constexpr auto m = 63.503_kg; // Robot mass + constexpr auto J = 5.6_kg_sq_m; // Robot moment of inertia + + auto C1 = -std::pow(Ghigh, 2) * motors.Kt / + (motors.Kv * motors.R * units::math::pow<2>(r)); + auto C2 = Ghigh * motors.Kt / (motors.R * r); + auto k1 = (1 / m + units::math::pow<2>(rb) / J); + auto k2 = (1 / m - units::math::pow<2>(rb) / J); + + units::meters_per_second_t vl{x(3)}; + units::meters_per_second_t vr{x(4)}; + units::volt_t Vl{u(0)}; + units::volt_t Vr{u(1)}; + + Eigen::Matrix result; + auto v = 0.5 * (vl + vr); + result(0) = v.to() * std::cos(x(2)); + result(1) = v.to() * std::sin(x(2)); + result(2) = ((vr - vl) / (2.0 * rb)).to(); + result(3) = + k1.to() * ((C1 * vl).to() + (C2 * Vl).to()) + + k2.to() * ((C1 * vr).to() + (C2 * Vr).to()); + result(4) = + k2.to() * ((C1 * vl).to() + (C2 * Vl).to()) + + k1.to() * ((C1 * vr).to() + (C2 * Vr).to()); + return result; +} + +Eigen::Matrix LocalMeasurementModel( + const Eigen::Matrix& x, + const Eigen::Matrix& u) { + static_cast(u); + Eigen::Matrix y; + y << x(2), x(3), x(4); + return y; +} + +Eigen::Matrix GlobalMeasurementModel( + const Eigen::Matrix& x, + const Eigen::Matrix& u) { + static_cast(u); + Eigen::Matrix y; + y << x(0), x(1), x(2), x(3), x(4); + return y; +} +} // namespace + +TEST(UnscentedKalmanFilterTest, Init) { + constexpr auto dt = 0.00505_s; + + frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics, + LocalMeasurementModel, + {0.5, 0.5, 10.0, 1.0, 1.0}, + {0.0001, 0.01, 0.01}, + dt}; + Eigen::Matrix u; + u << 12.0, 12.0; + observer.Predict(u, dt); + + auto localY = LocalMeasurementModel(observer.Xhat(), u); + observer.Correct(u, localY); + + auto globalY = GlobalMeasurementModel(observer.Xhat(), u); + auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01); + observer.Correct<5>(u, globalY, GlobalMeasurementModel, R); +} + +TEST(UnscentedKalmanFilterTest, Convergence) { + constexpr auto dt = 0.00505_s; + constexpr auto rb = 0.8382_m / 2.0; // Robot radius + + frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics, + LocalMeasurementModel, + {0.5, 0.5, 10.0, 1.0, 1.0}, + {0.0001, 0.5, 0.5}, + dt}; + + auto waypoints = + std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad}, + frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}}; + auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory( + waypoints, {8.8_mps, 0.1_mps_sq}); + + Eigen::Matrix r = Eigen::Matrix::Zero(); + + Eigen::Matrix nextR; + Eigen::Matrix u = Eigen::Matrix::Zero(); + + auto B = frc::NumericalJacobianU<5, 5, 2>( + Dynamics, Eigen::Matrix::Zero(), + Eigen::Matrix::Zero()); + + observer.SetXhat(frc::MakeMatrix<5, 1>( + trajectory.InitialPose().Translation().X().to(), + trajectory.InitialPose().Translation().Y().to(), + trajectory.InitialPose().Rotation().Radians().to(), 0.0, 0.0)); + + auto trueXhat = observer.Xhat(); + + auto totalTime = trajectory.TotalTime(); + for (size_t i = 0; i < (totalTime / dt).to(); ++i) { + auto ref = trajectory.Sample(dt * i); + units::meters_per_second_t vl = + ref.velocity * (1 - (ref.curvature * rb).to()); + units::meters_per_second_t vr = + ref.velocity * (1 + (ref.curvature * rb).to()); + + nextR(0) = ref.pose.Translation().X().to(); + nextR(1) = ref.pose.Translation().Y().to(); + nextR(2) = ref.pose.Rotation().Radians().to(); + nextR(3) = vl.to(); + nextR(4) = vr.to(); + + auto localY = + LocalMeasurementModel(trueXhat, Eigen::Matrix::Zero()); + observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5)); + + Eigen::Matrix rdot = (nextR - r) / dt.to(); + u = B.householderQr().solve( + rdot - Dynamics(r, Eigen::Matrix::Zero())); + + observer.Predict(u, dt); + + r = nextR; + trueXhat = frc::RungeKutta(Dynamics, trueXhat, u, dt); + } + + auto localY = LocalMeasurementModel(trueXhat, u); + observer.Correct(u, localY); + + auto globalY = GlobalMeasurementModel(trueXhat, u); + auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5); + observer.Correct<5>(u, globalY, GlobalMeasurementModel, R); + + auto finalPosition = trajectory.Sample(trajectory.TotalTime()); + ASSERT_NEAR(finalPosition.pose.Translation().X().template to(), + observer.Xhat(0), 1.0); + ASSERT_NEAR(finalPosition.pose.Translation().Y().template to(), + observer.Xhat(1), 1.0); + ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to(), + observer.Xhat(2), 1.0); + ASSERT_NEAR(0.0, observer.Xhat(3), 1.0); + ASSERT_NEAR(0.0, observer.Xhat(4), 1.0); +} diff --git a/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp new file mode 100644 index 0000000000..ef6b44dc30 --- /dev/null +++ b/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp @@ -0,0 +1,244 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include + +#include "Eigen/Core" +#include "frc/system/Discretization.h" +#include "frc/system/RungeKutta.h" + +// Check that for a simple second-order system that we can easily analyze +// analytically, +TEST(DiscretizationTest, DiscretizeA) { + Eigen::Matrix contA; + contA << 0, 1, 0, 0; + + Eigen::Matrix x0; + x0 << 1, 1; + Eigen::Matrix discA; + + frc::DiscretizeA<2>(contA, 1_s, &discA); + Eigen::Matrix x1Discrete = discA * x0; + + // We now have pos = vel = 1 and accel = 0, which should give us: + Eigen::Matrix x1Truth; + x1Truth(1) = x0(1); + x1Truth(0) = x0(0) + 1.0 * x0(1); + + EXPECT_EQ(x1Truth, x1Discrete); +} + +// Check that for a simple second-order system that we can easily analyze +// analytically, +TEST(DiscretizationTest, DiscretizeAB) { + Eigen::Matrix contA; + contA << 0, 1, 0, 0; + + Eigen::Matrix contB; + contB << 0, 1; + + Eigen::Matrix x0; + x0 << 1, 1; + Eigen::Matrix u; + u << 1; + Eigen::Matrix discA; + Eigen::Matrix discB; + + frc::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB); + Eigen::Matrix x1Discrete = discA * x0 + discB * u; + + // We now have pos = vel = accel = 1, which should give us: + Eigen::Matrix x1Truth; + x1Truth(1) = x0(1) + 1.0 * u(0); + x1Truth(0) = x0(0) + 1.0 * x0(1) + 0.5 * u(0); + + EXPECT_EQ(x1Truth, x1Discrete); +} + +// Test that the discrete approximation of Q is roughly equal to +// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau +TEST(DiscretizationTest, DiscretizeSlowModelAQ) { + Eigen::Matrix contA; + contA << 0, 1, 0, 0; + + Eigen::Matrix contQ; + contQ << 1, 0, 0, 1; + + constexpr auto dt = 1_s; + + Eigen::Matrix discQIntegrated = frc::RungeKuttaTimeVarying< + std::function( + units::second_t, const Eigen::Matrix&)>, + Eigen::Matrix>( + [&](units::second_t t, const Eigen::Matrix&) { + return Eigen::Matrix( + (contA * t.to()).exp() * contQ * + (contA.transpose() * t.to()).exp()); + }, + Eigen::Matrix::Zero(), 0_s, dt); + + Eigen::Matrix discA; + Eigen::Matrix discQ; + frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ); + + EXPECT_LT((discQIntegrated - discQ).norm(), 1e-10) + << "Expected these to be nearly equal:\ndiscQ:\n" + << discQ << "\ndiscQIntegrated:\n" + << discQIntegrated; +} + +// Test that the discrete approximation of Q is roughly equal to +// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau +TEST(DiscretizationTest, DiscretizeFastModelAQ) { + Eigen::Matrix contA; + contA << 0, 1, 0, -1406.29; + + Eigen::Matrix contQ; + contQ << 0.0025, 0, 0, 1; + + constexpr auto dt = 5.05_ms; + + Eigen::Matrix discQIntegrated = frc::RungeKuttaTimeVarying< + std::function( + units::second_t, const Eigen::Matrix&)>, + Eigen::Matrix>( + [&](units::second_t t, const Eigen::Matrix&) { + return Eigen::Matrix( + (contA * t.to()).exp() * contQ * + (contA.transpose() * t.to()).exp()); + }, + Eigen::Matrix::Zero(), 0_s, dt); + + Eigen::Matrix discA; + Eigen::Matrix discQ; + frc::DiscretizeAQ<2>(contA, contQ, dt, &discA, &discQ); + + EXPECT_LT((discQIntegrated - discQ).norm(), 1e-3) + << "Expected these to be nearly equal:\ndiscQ:\n" + << discQ << "\ndiscQIntegrated:\n" + << discQIntegrated; +} + +// Test that the Taylor series discretization produces nearly identical results. +TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) { + Eigen::Matrix contA; + contA << 0, 1, 0, 0; + + Eigen::Matrix contB; + contB << 0, 1; + + Eigen::Matrix contQ; + contQ << 1, 0, 0, 1; + + constexpr auto dt = 1_s; + + Eigen::Matrix discQTaylor; + Eigen::Matrix discA; + Eigen::Matrix discATaylor; + Eigen::Matrix discB; + + // Continuous Q should be positive semidefinite + Eigen::SelfAdjointEigenSolver esCont(contQ); + for (int i = 0; i < contQ.rows(); i++) { + EXPECT_GT(esCont.eigenvalues()[i], 0); + } + + Eigen::Matrix discQIntegrated = frc::RungeKuttaTimeVarying< + std::function( + units::second_t, const Eigen::Matrix&)>, + Eigen::Matrix>( + [&](units::second_t t, const Eigen::Matrix&) { + return Eigen::Matrix( + (contA * t.to()).exp() * contQ * + (contA.transpose() * t.to()).exp()); + }, + Eigen::Matrix::Zero(), 0_s, dt); + + frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB); + frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor); + + EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10) + << "Expected these to be nearly equal:\ndiscQTaylor:\n" + << discQTaylor << "\ndiscQIntegrated:\n" + << discQIntegrated; + EXPECT_LT((discA - discATaylor).norm(), 1e-10); + + // Discrete Q should be positive semidefinite + Eigen::SelfAdjointEigenSolver esDisc(discQTaylor); + for (int i = 0; i < discQTaylor.rows(); i++) { + EXPECT_GT(esDisc.eigenvalues()[i], 0); + } +} + +// Test that the Taylor series discretization produces nearly identical results. +TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) { + Eigen::Matrix contA; + contA << 0, 1, 0, -1500; + + Eigen::Matrix contB; + contB << 0, 1; + + Eigen::Matrix contQ; + contQ << 0.0025, 0, 0, 1; + + constexpr auto dt = 5.05_ms; + + Eigen::Matrix discQTaylor; + Eigen::Matrix discA; + Eigen::Matrix discATaylor; + Eigen::Matrix discB; + + // Continuous Q should be positive semidefinite + Eigen::SelfAdjointEigenSolver esCont(contQ); + for (int i = 0; i < contQ.rows(); i++) { + EXPECT_GT(esCont.eigenvalues()[i], 0); + } + + Eigen::Matrix discQIntegrated = frc::RungeKuttaTimeVarying< + std::function( + units::second_t, const Eigen::Matrix&)>, + Eigen::Matrix>( + [&](units::second_t t, const Eigen::Matrix&) { + return Eigen::Matrix( + (contA * t.to()).exp() * contQ * + (contA.transpose() * t.to()).exp()); + }, + Eigen::Matrix::Zero(), 0_s, dt); + + frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB); + frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor); + + EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3) + << "Expected these to be nearly equal:\ndiscQTaylor:\n" + << discQTaylor << "\ndiscQIntegrated:\n" + << discQIntegrated; + EXPECT_LT((discA - discATaylor).norm(), 1e-10); + + // Discrete Q should be positive semidefinite + Eigen::SelfAdjointEigenSolver esDisc(discQTaylor); + for (int i = 0; i < discQTaylor.rows(); i++) { + EXPECT_GT(esDisc.eigenvalues()[i], 0); + } +} + +// Test that DiscretizeR() works +TEST(DiscretizationTest, DiscretizeR) { + Eigen::Matrix contR; + contR << 2.0, 0.0, 0.0, 1.0; + + Eigen::Matrix discRTruth; + discRTruth << 4.0, 0.0, 0.0, 2.0; + + Eigen::Matrix discR = frc::DiscretizeR<2>(contR, 500_ms); + + EXPECT_LT((discRTruth - discR).norm(), 1e-10) + << "Expected these to be nearly equal:\ndiscR:\n" + << discR << "\ndiscRTruth:\n" + << discRTruth; +} diff --git a/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp new file mode 100644 index 0000000000..60d2a4ed6a --- /dev/null +++ b/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp @@ -0,0 +1,77 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include +#include +#include +#include + +#include + +#include "frc/StateSpaceUtil.h" +#include "frc/system/plant/LinearSystemId.h" +#include "units/length.h" +#include "units/mass.h" + +TEST(LinearSystemIDTest, IdentifyDrivetrainVelocitySystem) { + auto model = frc::LinearSystemId::DrivetrainVelocitySystem( + frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0); + + ASSERT_TRUE(model.A().isApprox( + frc::MakeMatrix<2, 2>(-10.14132, 3.06598, 3.06598, -10.14132), 0.001)); + ASSERT_TRUE(model.B().isApprox( + frc::MakeMatrix<2, 2>(4.2590, -1.28762, -1.2876, 4.2590), 0.001)); + ASSERT_TRUE( + model.C().isApprox(frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0), 0.001)); + ASSERT_TRUE( + model.D().isApprox(frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0), 0.001)); +} + +TEST(LinearSystemIDTest, ElevatorSystem) { + auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg, + 0.05_m, 12); + ASSERT_TRUE(model.A().isApprox( + frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -99.05473), 0.001)); + ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<2, 1>(0.0, 20.8), 0.001)); + ASSERT_TRUE(model.C().isApprox(frc::MakeMatrix<1, 2>(1.0, 0.0), 0.001)); + ASSERT_TRUE(model.D().isApprox(frc::MakeMatrix<1, 1>(0.0), 0.001)); +} + +TEST(LinearSystemIDTest, FlywheelSystem) { + auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2), + 0.00032_kg_sq_m, 1.0); + ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-26.87032), 0.001)); + ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1354.166667), 0.001)); + ASSERT_TRUE(model.C().isApprox(frc::MakeMatrix<1, 1>(1.0), 0.001)); + ASSERT_TRUE(model.D().isApprox(frc::MakeMatrix<1, 1>(0.0), 0.001)); +} + +TEST(LinearSystemIDTest, IdentifyPositionSystem) { + // By controls engineering in frc, + // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u + double kv = 1.0; + double ka = 0.5; + auto model = frc::LinearSystemId::IdentifyPositionSystem(kv, ka); + + ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kv / ka), + 0.001)); + ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<2, 1>(0.0, 1.0 / ka), 0.001)); +} + +TEST(LinearSystemIDTest, IdentifyVelocitySystem) { + // By controls engineering in frc, + // V = kv * velocity + ka * acceleration + // x-dot = -kv/ka * v + 1/ka \cdot V + double kv = 1.0; + double ka = 0.5; + auto model = frc::LinearSystemId::IdentifyVelocitySystem(kv, ka); + + std::cout << model.A() << std::endl << model.B() << std::endl; + + ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-kv / ka), 0.001)); + ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1.0 / ka), 0.001)); +} diff --git a/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp new file mode 100644 index 0000000000..ddc3f68b0a --- /dev/null +++ b/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp @@ -0,0 +1,68 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include "frc/system/NumericalJacobian.h" + +Eigen::Matrix A = (Eigen::Matrix() << 1, 2, 4, 1, 5, + 2, 3, 4, 5, 1, 3, 2, 1, 1, 3, 7) + .finished(); + +Eigen::Matrix B = + (Eigen::Matrix() << 1, 1, 2, 1, 3, 2, 3, 7).finished(); + +// Function from which to recover A and B +Eigen::Matrix AxBuFn(const Eigen::Matrix& x, + const Eigen::Matrix& u) { + return A * x + B * u; +} + +// Test that we can recover A from AxBuFn() pretty accurately +TEST(NumericalJacobianTest, Ax) { + Eigen::Matrix newA = frc::NumericalJacobianX<4, 4, 2>( + AxBuFn, Eigen::Matrix::Zero(), + Eigen::Matrix::Zero()); + EXPECT_TRUE(newA.isApprox(A)); +} + +// Test that we can recover B from AxBuFn() pretty accurately +TEST(NumericalJacobianTest, Bu) { + Eigen::Matrix newB = frc::NumericalJacobianU<4, 4, 2>( + AxBuFn, Eigen::Matrix::Zero(), + Eigen::Matrix::Zero()); + EXPECT_TRUE(newB.isApprox(B)); +} + +Eigen::Matrix C = + (Eigen::Matrix() << 1, 2, 4, 1, 5, 2, 3, 4, 5, 1, 3, 2) + .finished(); + +Eigen::Matrix D = + (Eigen::Matrix() << 1, 1, 2, 1, 3, 2).finished(); + +// Function from which to recover C and D +Eigen::Matrix CxDuFn(const Eigen::Matrix& x, + const Eigen::Matrix& u) { + return C * x + D * u; +} + +// Test that we can recover C from CxDuFn() pretty accurately +TEST(NumericalJacobianTest, Cx) { + Eigen::Matrix newC = frc::NumericalJacobianX<3, 4, 2>( + CxDuFn, Eigen::Matrix::Zero(), + Eigen::Matrix::Zero()); + EXPECT_TRUE(newC.isApprox(C)); +} + +// Test that we can recover D from CxDuFn() pretty accurately +TEST(NumericalJacobianTest, Du) { + Eigen::Matrix newD = frc::NumericalJacobianU<3, 4, 2>( + CxDuFn, Eigen::Matrix::Zero(), + Eigen::Matrix::Zero()); + EXPECT_TRUE(newD.isApprox(D)); +} diff --git a/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp b/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp new file mode 100644 index 0000000000..a12c1b7565 --- /dev/null +++ b/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019-2020 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#include + +#include + +#include "frc/system/RungeKutta.h" + +// Tests that integrating dx/dt = e^x works. +TEST(RungeKuttaTest, Exponential) { + Eigen::Matrix y0; + y0(0) = 0.0; + + Eigen::Matrix y1 = frc::RungeKutta( + [](Eigen::Matrix x) { + Eigen::Matrix y; + y(0) = std::exp(x(0)); + return y; + }, + y0, 0.1_s); + EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); +} + +// Tests that integrating dx/dt = e^x works when we provide a U. +TEST(RungeKuttaTest, ExponentialWithU) { + Eigen::Matrix y0; + y0(0) = 0.0; + + Eigen::Matrix y1 = frc::RungeKutta( + [](Eigen::Matrix x, Eigen::Matrix u) { + Eigen::Matrix y; + y(0) = std::exp(u(0) * x(0)); + return y; + }, + y0, (Eigen::Matrix() << 1.0).finished(), 0.1_s); + EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3); +} + +namespace { +Eigen::Matrix RungeKuttaTimeVaryingSolution(double t) { + return (Eigen::Matrix() + << 12.0 * std::exp(t) / (std::pow(std::exp(t) + 1.0, 2.0))) + .finished(); +} +} // namespace + +// Tests RungeKutta with a time varying solution. +// Now, lets test RK4 with a time varying solution. From +// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html: +// x' = x (2 / (e^t + 1) - 1) +// +// The true (analytical) solution is: +// +// x(t) = 12 * e^t / ((e^t + 1)^2) +TEST(RungeKuttaTest, RungeKuttaTimeVarying) { + Eigen::Matrix y0 = RungeKuttaTimeVaryingSolution(5.0); + + Eigen::Matrix y1 = frc::RungeKuttaTimeVarying( + [](units::second_t t, Eigen::Matrix x) { + return (Eigen::Matrix() + << x(0) * (2.0 / (std::exp(t.to()) + 1.0) - 1.0)) + .finished(); + }, + y0, 5_s, 1_s); + EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3); +}