CNR-STIIMA-IRAS/cnr_control_toolbox

CNR Control Toolbox

Aim

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.

Package Organization

Classes Available

DiscreteStateSpace is generic discrete state space linear system

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)

USAGE

Basic Usage

#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

Loading the Matrices from Param

#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]  

FirstOrderLowPass and FirstOrderHighPass are low-pass and high-pass first-order filters

Low-pass filter: discretized version of 1/(tau*s+1)
High-pass filter: discretized version of tau*s/(tau*s+1)

Usage of the FirstOrderLowPass

#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);
   

Load from params

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.

Contribution guidelines

Contact

<mailto:mailto:manuel.beschi@stiima.cnr.it> <mailto:mailto:nicola.pedrocchi@stiima.cnr.it>