Skip to content

Commit fd7f546

Browse files
markusgftGiftthaler Markus (CR/PJ-AI-R31)
markusgft
authored and
Giftthaler Markus (CR/PJ-AI-R31)
committed
moving a class for input disturbed system into separate header.
1 parent 7ac6fc8 commit fd7f546

File tree

5 files changed

+134
-58
lines changed

5 files changed

+134
-58
lines changed

‎ct_optcon/examples/KalmanDisturbanceFiltering.cpp

Lines changed: 6 additions & 58 deletions
Original file line numberDiff line numberDiff line change
@@ -85,59 +85,6 @@ class CustomController : public ct::core::Controller<state_dim, control_dim>
8585
};
8686

8787

88-
/*!
89-
* Implementation of the "DisturbedSystem" which is going to be used for handed over to the Kalman Filter
90-
* for dynamics prediction and computing derivatives.
91-
* @note this system is not used for simulation, but for filtering.
92-
*/
93-
template <size_t state_dim, size_t dist_dim, size_t control_dim, typename SCALAR = double>
94-
class CustomDisturbedSystem : public ct::optcon::DisturbedSystem<state_dim, dist_dim, control_dim, SCALAR>
95-
{
96-
public:
97-
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
98-
99-
//! constructor
100-
CustomDisturbedSystem(std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim, SCALAR>> sys)
101-
: ct::optcon::DisturbedSystem<state_dim, dist_dim, control_dim, SCALAR>(sys->getController()), system_(sys)
102-
{
103-
}
104-
105-
//! copy constructor
106-
CustomDisturbedSystem(const CustomDisturbedSystem& other)
107-
: ct::optcon::DisturbedSystem<state_dim, dist_dim, control_dim, SCALAR>(*this), system_(other.system_->clone())
108-
{
109-
}
110-
111-
//! deep cloning
112-
CustomDisturbedSystem* clone() const override { return new CustomDisturbedSystem(*this); }
113-
114-
/*!
115-
* Override the computeControlledDynamics() with a custom update rule.
116-
*/
117-
void computeControlledDynamics(const ct::core::StateVector<state_dim + dist_dim, SCALAR>& state,
118-
const SCALAR& t,
119-
const ct::core::ControlVector<control_dim, SCALAR>& control,
120-
ct::core::StateVector<state_dim + dist_dim, SCALAR>& derivative) override
121-
{
122-
derivative.setZero();
123-
ct::core::StateVector<state_dim, SCALAR> tempDerivative;
124-
125-
// the control consists of actual commanded control plus the estimated input disturbance,
126-
// which is the augmented part of the state vector
127-
ct::core::ControlVector<control_dim, SCALAR> disturbed_control = control + state.template tail<dist_dim>();
128-
129-
// the dynamics of the augmented system
130-
system_->computeControlledDynamics(state.head(state_dim), t, disturbed_control, tempDerivative);
131-
derivative.template head<state_dim>() = tempDerivative;
132-
}
133-
134-
135-
private:
136-
// the nominal system (the one we are trying to control)
137-
std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim, SCALAR>> system_;
138-
};
139-
140-
14188
int main(int argc, char** argv)
14289
{
14390
// file with weights and settings
@@ -215,8 +162,8 @@ int main(int argc, char** argv)
215162
std::shared_ptr<CustomController> controller_nominal(
216163
new CustomController(uff_magnitude, uff_frequency, kp, kd, 0.0, 0.0));
217164

218-
std::shared_ptr<CustomDisturbedSystem<state_dim, dist_dim, control_dim>> customdisturbedSystem(
219-
new CustomDisturbedSystem<state_dim, dist_dim, control_dim>(oscillator_obs));
165+
std::shared_ptr<ct::optcon::InputDisturbedSystem<state_dim, control_dim>> inputDisturbedSystem(
166+
new ct::optcon::InputDisturbedSystem<state_dim, control_dim>(oscillator_obs));
220167

221168
// Observation matrix for the state
222169
ct::core::OutputStateMatrix<output_dim, state_dim> C;
@@ -240,12 +187,12 @@ int main(int argc, char** argv)
240187

241188
// create a sensitivity approximator to obtain discrete-time dynamics matrices
242189
std::shared_ptr<ct::core::SystemLinearizer<state_dim + dist_dim, control_dim>> linearizer(
243-
new ct::core::SystemLinearizer<state_dim + dist_dim, control_dim>(customdisturbedSystem));
190+
new ct::core::SystemLinearizer<state_dim + dist_dim, control_dim>(inputDisturbedSystem));
244191
ct::core::SensitivityApproximation<state_dim + dist_dim, control_dim> sensApprox(dt, linearizer);
245192

246193
// set up the system model
247194
std::shared_ptr<ct::optcon::CTSystemModel<state_dim + dist_dim, control_dim>> sysModel(
248-
new ct::optcon::CTSystemModel<state_dim + dist_dim, control_dim>(customdisturbedSystem, sensApprox, dFdv));
195+
new ct::optcon::CTSystemModel<state_dim + dist_dim, control_dim>(inputDisturbedSystem, sensApprox, dFdv));
249196

250197
// set up the measurement model
251198
std::shared_ptr<ct::optcon::LinearMeasurementModel<output_dim, state_dim + dist_dim>> measModel(
@@ -273,7 +220,8 @@ int main(int argc, char** argv)
273220

274221

275222
// set up Extended Kalman Filter
276-
ct::optcon::ExtendedKalmanFilter<state_dim + dist_dim, control_dim, output_dim> ekf(sysModel, measModel, Qaug, R, x0aug, Qaug);
223+
ct::optcon::ExtendedKalmanFilter<state_dim + dist_dim, control_dim, output_dim> ekf(
224+
sysModel, measModel, Qaug, R, x0aug, Qaug);
277225

278226

279227
// run the filter over the simulated data

‎ct_optcon/include/ct/optcon/filter/InputDisturbedSystem-impl.h

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
2+
/**********************************************************************************************************************
3+
This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
4+
Licensed under Apache2 license (see LICENSE file in main directory)
5+
**********************************************************************************************************************/
6+
7+
#pragma once
8+
9+
namespace ct {
10+
namespace optcon {
11+
12+
template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
13+
InputDisturbedSystem<STATE_DIM, CONTROL_DIM, SCALAR>::InputDisturbedSystem(
14+
std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> sys)
15+
: Base(sys->getController()), system_(sys)
16+
{
17+
}
18+
19+
template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
20+
InputDisturbedSystem<STATE_DIM, CONTROL_DIM, SCALAR>::InputDisturbedSystem(const InputDisturbedSystem& other)
21+
: Base(*this), system_(other.system_->clone())
22+
{
23+
}
24+
25+
template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
26+
InputDisturbedSystem<STATE_DIM, CONTROL_DIM, SCALAR>* InputDisturbedSystem<STATE_DIM, CONTROL_DIM, SCALAR>::clone()
27+
const
28+
{
29+
return new InputDisturbedSystem(*this);
30+
}
31+
32+
template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR>
33+
void InputDisturbedSystem<STATE_DIM, CONTROL_DIM, SCALAR>::computeControlledDynamics(
34+
const ct::core::StateVector<AUGMENTED_STATE_DIM, SCALAR>& state,
35+
const SCALAR& t,
36+
const ct::core::ControlVector<CONTROL_DIM, SCALAR>& control,
37+
ct::core::StateVector<AUGMENTED_STATE_DIM, SCALAR>& derivative)
38+
{
39+
// the derivative of the original, non-augmented system
40+
ct::core::StateVector<STATE_DIM, SCALAR> tempDerivative;
41+
42+
// the control consists of actual commanded control plus the estimated input disturbance,
43+
// which is the augmented part of the state vector
44+
ct::core::ControlVector<CONTROL_DIM, SCALAR> disturbed_control = control + state.template tail<CONTROL_DIM>();
45+
46+
// the dynamics of the augmented system consist of the original dynamics ...
47+
system_->computeControlledDynamics(state.template head<STATE_DIM>(), t, disturbed_control, tempDerivative);
48+
derivative.template head<STATE_DIM>() = tempDerivative;
49+
50+
// and the disturbance dynamics, which is zero as the disturbance is assumed constant.
51+
derivative.template tail<CONTROL_DIM>().setZero();
52+
}
53+
54+
} // namespace optcon
55+
} // namespace ct

‎ct_optcon/include/ct/optcon/filter/InputDisturbedSystem.h

Lines changed: 71 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
/**********************************************************************************************************************
2+
This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
3+
Licensed under Apache2 license (see LICENSE file in main directory)
4+
**********************************************************************************************************************/
5+
6+
#pragma once
7+
8+
#include "DisturbedSystem.h"
9+
10+
namespace ct {
11+
namespace optcon {
12+
13+
/**
14+
* @brief Implementation of an input disturbed system where, the dimension of the disturbance is equal to the
15+
* dimension of the control input, thus DIST_DIM = CONTROL_DIM. This is a special case, however it occurs often
16+
* and is convenient to have as separate class.
17+
*
18+
* The new state vector has the form x_aug = [x_system + disturbance];
19+
*
20+
* @note this system should not be for simulation, but for filtering. It is not suitable for simulation since the
21+
* disturbance state gets mapped back to the control input.
22+
*
23+
* @tparam STATE_DIM state dimension
24+
* @tparam CONTROL_DIM
25+
* @tparam double
26+
*/
27+
template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR = double>
28+
class InputDisturbedSystem : public ct::optcon::DisturbedSystem<STATE_DIM, CONTROL_DIM, CONTROL_DIM, SCALAR>
29+
{
30+
public:
31+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
32+
33+
// the dimension of the augmented state used in the filter
34+
static const size_t AUGMENTED_STATE_DIM = STATE_DIM + CONTROL_DIM;
35+
36+
using Base = typename ct::optcon::DisturbedSystem<STATE_DIM, CONTROL_DIM, CONTROL_DIM, SCALAR>;
37+
38+
/**
39+
* @brief Construct a new Input Disturbed System object
40+
*
41+
* @param sys the system that is subject to a disturbance
42+
*/
43+
InputDisturbedSystem(std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> sys);
44+
45+
//! copy constructor
46+
InputDisturbedSystem(const InputDisturbedSystem& other);
47+
48+
//! deep cloning
49+
InputDisturbedSystem* clone() const override;
50+
51+
/**
52+
* @brief compute the dynamics (the left-hand-side of the dynamics equation) for the disturbance-augmented system
53+
*
54+
* @param state the augmented state
55+
* @param t the current time
56+
* @param control the nominal control (non-perturbed)
57+
* @param derivative the left-hand side of the augmented system
58+
*/
59+
void computeControlledDynamics(const ct::core::StateVector<AUGMENTED_STATE_DIM, SCALAR>& state,
60+
const SCALAR& t,
61+
const ct::core::ControlVector<CONTROL_DIM, SCALAR>& control,
62+
ct::core::StateVector<AUGMENTED_STATE_DIM, SCALAR>& derivative) override;
63+
64+
private:
65+
// the nominal system (the one we are trying to control)
66+
std::shared_ptr<ct::core::ControlledSystem<STATE_DIM, CONTROL_DIM, SCALAR>> system_;
67+
};
68+
69+
} // namespace optcon
70+
} // namespace ct
71+

‎ct_optcon/include/ct/optcon/filter/filter-impl.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@ Licensed under Apache2 license (see LICENSE file in main directory)
77

88
#include "CTSystemModel-impl.h"
99
#include "DisturbedSystem-impl.h"
10+
#include "InputDisturbedSystem-impl.h"
1011
#include "DisturbedSystemController-impl.h"
1112
#include "LTIMeasurementModel-impl.h"
1213
#include "ExtendedKalmanFilter-impl.h"

‎ct_optcon/include/ct/optcon/filter/filter.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@ Licensed under Apache2 license (see LICENSE file in main directory)
88
#include "CTSystemModel.h"
99
#include "DisturbedSystem.h"
1010
#include "DisturbedSystemController.h"
11+
#include "InputDisturbedSystem.h"
1112
#include "ExtendedKalmanFilter.h"
1213
#include "EstimatorBase.h"
1314
#include "FilterSettings.h"

0 commit comments

Comments
 (0)