1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <array>
#include <units/current.h>
#include <units/time.h>
#include "frc/EigenCore.h"
#include "frc/StateSpaceUtil.h"
#include "frc/system/LinearSystem.h"
namespace frc::sim {
/**
* This class helps simulate linear systems. To use this class, do the following
* in the simulationPeriodic() method.
*
* Call the SetInput() method with the inputs to your system (generally
* voltage). Call the Update() method to update the simulation. Set simulated
* sensor readings with the simulated positions in the GetOutput() method.
*
* @tparam States Number of states of the system.
* @tparam Inputs Number of inputs to the system.
* @tparam Outputs Number of outputs of the system.
*/
template <int States, int Inputs, int Outputs>
class LinearSystemSim {
public:
/**
* Creates a simulated generic linear system.
*
* @param system The system to simulate.
* @param measurementStdDevs The standard deviations of the measurements.
*/
explicit LinearSystemSim(
const LinearSystem<States, Inputs, Outputs>& system,
const std::array<double, Outputs>& measurementStdDevs = {})
: m_plant(system), m_measurementStdDevs(measurementStdDevs) {
m_x = Vectord<States>::Zero();
m_y = Vectord<Outputs>::Zero();
m_u = Vectord<Inputs>::Zero();
}
virtual ~LinearSystemSim() = default;
/**
* Updates the simulation.
*
* @param dt The time between updates.
*/
void Update(units::second_t dt) {
// Update x. By default, this is the linear system dynamics xₖ₊₁ = Axₖ +
// Buₖ.
m_x = UpdateX(m_x, m_u, dt);
// yₖ = Cxₖ + Duₖ
m_y = m_plant.CalculateY(m_x, m_u);
// Add noise. If the user did not pass a noise vector to the
// constructor, then this method will not do anything because
// the standard deviations default to zero.
m_y += frc::MakeWhiteNoiseVector<Outputs>(m_measurementStdDevs);
}
/**
* Returns the current output of the plant.
*
* @return The current output of the plant.
*/
const Vectord<Outputs>& GetOutput() const { return m_y; }
/**
* Returns an element of the current output of the plant.
*
* @param row The row to return.
* @return An element of the current output of the plant.
*/
double GetOutput(int row) const { return m_y(row); }
/**
* Sets the system inputs (usually voltages).
*
* @param u The system inputs.
*/
void SetInput(const Vectord<Inputs>& u) { m_u = u; }
/**
* Sets the system inputs.
*
* @param row The row in the input matrix to set.
* @param value The value to set the row to.
*/
void SetInput(int row, double value) { m_u(row, 0) = value; }
/**
* Returns the current input of the plant.
*
* @return The current input of the plant.
*/
const Vectord<Inputs>& GetInput() const { return m_u; }
/**
* Returns an element of the current input of the plant.
*
* @param row The row to return.
* @return An element of the current input of the plant.
*/
double GetInput(int row) const { return m_u(row); }
/**
* Sets the system state.
*
* @param state The new state.
*/
void SetState(const Vectord<States>& state) {
m_x = state;
// Update the output to reflect the new state.
//
// yₖ = Cxₖ + Duₖ
m_y = m_plant.CalculateY(m_x, m_u);
}
protected:
/**
* Updates the state estimate of the system.
*
* @param currentXhat The current state estimate.
* @param u The system inputs (usually voltage).
* @param dt The time difference between controller updates.
*/
virtual Vectord<States> UpdateX(const Vectord<States>& currentXhat,
const Vectord<Inputs>& u,
units::second_t dt) {
return m_plant.CalculateX(currentXhat, u, dt);
}
/**
* Clamp the input vector such that no element exceeds the given voltage. If
* any does, the relative magnitudes of the input will be maintained.
*
* @param maxInput The maximum magnitude of the input vector after clamping.
*/
void ClampInput(double maxInput) {
m_u = frc::DesaturateInputVector<Inputs>(m_u, maxInput);
}
/// The plant that represents the linear system.
LinearSystem<States, Inputs, Outputs> m_plant;
/// State vector.
Vectord<States> m_x;
/// Input vector.
Vectord<Inputs> m_u;
/// Output vector.
Vectord<Outputs> m_y;
/// The standard deviations of measurements, used for adding noise to the
/// measurements.
std::array<double, Outputs> m_measurementStdDevs;
};
} // namespace frc::sim