state_space_systems is an Eigen implementation of a discrete state space linear system, including special case like: low- and high-pass first-order filters.
x state
y output
u input
k actual step
y(k)=C*x(k)+D*u(k)
x(k+1)=A*x(k)+B*u(k)
#include <state_space_systems/discrete_state_space_systems.h>
unsigned int order=10; // system order
unsigned int nin=1; // number of inputs
unsigned int nout=1; // number of outputs
Eigen::MatrixXd A(order,order);
Eigen::MatrixXd B(order,nin);
Eigen::MatrixXd C(nout,order);
Eigen::MatrixXd D(nout,nin);
A.setRandom();
B.setRandom();
C.setRandom();
D.setRandom();
eigen_control_toolbox::DiscreteStateSpace ss(A,B,C,D);
Eigen::VectorXd u(nin); //input vector
Eigen::VectorXd y(nout); //output vector
u.setRandom();
y.setRandom();
ss.setStateFromLastIO(u,y); // initialize initial state value for dumpless startup
ROS_INFO_STREAM("state:\n"<<ss.getState());
ROS_INFO_STREAM("output:\n"<<ss.getOutput() << "\ndesired:\n"<<y);
y=ss.update(u); // computing one step, updating state and output
#include <state_space_systems/discrete_state_space_systems.h>
unsigned int order=10; // system order
unsigned int nin=1; // number of inputs
unsigned int nout=1; // number of outputs
eigen_control_toolbox::DiscreteStateSpace ss;
if (!ss.importMatricesFromParam(nh,"ss")) // reading matrices from ss parameter (see below)
{
ROS_ERROR("error");
return -1;
}
Eigen::VectorXd u(nin); //input vector
Eigen::VectorXd y(nout); //output vector
u.setRandom();
y.setRandom();
ss.setStateFromLastIO(u,y); // initialize initial state value for dumpless startup
ROS_INFO_STREAM("state:\n"<<ss.getState());
ROS_INFO_STREAM("output:\n"<<ss.getOutput() << "\ndesired:\n"<<y);
y=ss.update(u); // computing one step, updating state and output
Required parameters:
ss:
A:
- [0, 1]
- [0, 0]
B:
- [0]
- [1]
C:
- [1, 0]
D:
- [0]
Low-pass filter: discretized version of 1/(tau*s+1)
High-pass filter: discretized version of tau*s/(tau*s+1)
#include <state_space_systems/eigen_common_filters.h>
double natural_frequency = 500; // [rad/s]
double sampling_period=0.001; // s
eigen_control_toolbox::FirstOrderLowPassX lpf(natural_frequency,sampling_period); // the same for FirstOrderHighPass
// initialization
double u=0;
double y=0;
lpf.setStateFromLastIO(u, y);
// computing one step
u=1;
y=lpf.update(u);
you can load from param with the command:
eigen_control_toolbox::FirstOrderLowPassX lpf;
lpf.importMatricesFromParam(nh,"/filter");
The ROS parameter can be equal to:
filter:
frequency: 5 # [Hz]
sampling_period: 0.01 # [s]
or equal to:
filter:
natural_frequency: 2 # [rad/s]
sampling_period: 0.01 # [s]
Software License Agreement (BSD License)
Copyright (c) 2010, National Research Council of Italy, Institute of Intelligent Industrial Technologies and Systems for Advanced Manufacturing
All rights reserved.
<mailto:mailto:manuel.beschi@stiima.cnr.it> <mailto:mailto:nicola.pedrocchi@stiima.cnr.it>