Simulating State-Space Models in Eigen C++ Matrix Library – Object-Oriented Program – Fusion of Engineering, Control, Coding, Machine Learning, and Science

In this post, we explain how to simulate a state-space model of a dynamical system in the Eigen C++ matrix library. We have developed a C++ class for simulating the dynamical system. The developed program has the ability to simulate the dynamics from specified initial conditions and for specified input sequences. Also, we compare the Eigen C++ simulation with simulation performed in MATLAB. The video accompanying and supporting this post is given below. The project GitHub page is provided here.

First, we explain the class of systems we want to simulate. We consider the discrete-time state-space model

(1) $\begin{align*}& \mathbf{x}_{k+1}=A\mathbf{x}_{k}+B\mathbf{u}_{k} \\& \mathbf{y}_{k}=C\mathbf{x}_{k}\end{align*}$

where $\mathbf{x}\in \mathbb{R}^{n}$ is the state vector, $\mathbf{u}_{k}\in \mathbb{R}^{m}$ is the input vector, and $\mathbf{y}_{k}\in \mathbb{R}^{r}$ is the output vector. The matrices $A\in \mathbb{R}^{n\times n}$ , $B\in \mathbb{R}^{n\times m}$ , and $C\in \mathbb{R}^{r\times n}$ are the system matrices characterizing the dynamics of the state-space model. The state-space model is in discrete-time. This model can be obtained by discretizing the continuous-time model by using the backward Euler method. The discretization procedure is explained in our previous post.

To simulate the state-space model we need to know the initial state $\mathbf{x}_{0}$ and the sequence of the input-data $\{ \mathbf{u}_{0},\mathbf{u}_{1},\ldots, \mathbf{u}_{N-1} \}$ , where $N$ is the number of time samples.

Our strategy for simulating the state-space model using the Eigen matrix library is to create a C++ class object whose private member variables specify the dynamics, simulation conditions, and simulated states and outputs. Furthermore, this object has the ability to load and save matrices and simulation data from and in CSV files. In our previous post, we developed functions for storing and loading Eigen C++ matrix data to and from CSV files. The post can be found here.

Here is the header file of the class for simulating the state-space model.

// Class for simulating a linear state-space model
//  x_{k+1}=Ax_{k}+Bu_{k}
//  y_{k} = Cx_{k}
//  starting from the initial state x0;
// The class is implemented using the Eigen library:
// http://eigen.tuxfamily.org/index.php?title=Main_Page
//*******************************************************************************************************************************
// AUTHOR: Aleksandar Haber,
// DEVELOPMENT: June 2020-
// VERSION DATE: June 25, 2020
// NOTE: This implementation is not optimized for extremely large-scale problems, since all the matrices are passed by value 
//*******************************************************************************************************************************

#ifndef SIMULATESYSTEM_H
#define SIMULATESYSTEM_H
#include <iostream>
#include<Eigen/Dense>
#include<tuple>
#include<string>
#include<fstream>
#include<vector>

using namespace Eigen;

// MatrixXd is an Eigen typdef for Matrix<double, Dynamic, Dynamic>


class SimulateSystem {
public:
	
	SimulateSystem();
	// default constructor
	// sets all the variables to 1x1 dimensional matrices and sets all the variables to zero
	
	SimulateSystem(MatrixXd Amatrix, MatrixXd Bmatrix, MatrixXd Cmatrix, MatrixXd initialState, MatrixXd inputSequenceMatrix);
	// overloaded constructor assigns all the private variables 


	~SimulateSystem();
	// Default destructor  - currently just an empty implementation in the ".cpp" file
	
	// MatrixXd is an Eigen typdef for Matrix<double, Dynamic, Dynamic>
	std::tuple<MatrixXd, MatrixXd, MatrixXd> getStateOuputTime();
	//this function returns the simulated state and output sequences, as well as the time row vector used for simulation

	void runSimulation();
	// function that simulates the system 

	void saveData(std::string AFile, std::string BFile, std::string CFile, std::string x0File, std::string inputSequenceFile, std::string simulatedStateSequenceFile, std::string simulatedOutputSequenceFile) const;
	// this function saves the data in "*.csv" files
	

	MatrixXd openData(std::string fileToOpen);
	// this function opens the "*.csv" file "fileToOpen" that stores a matrix, and loads the entries into the Eigen matrix MatrixXd
	
	void openFromFile(std::string Afile, std::string Bfile, std::string Cfile, std::string x0File, std::string inputSequenceFile);

	// this function assigns the A,B,C,x0, inputSequence variables using the information stored in the corresponding files 
	// this function calls the function MatrixXd openData(std::string fileToOpen);
	// this function acts as a constructor in some way. 
	// call this function after a default constructor is being called
	// the inspiration for creating this function was drawn from here (I did NOT copy and paste the code)
	// https://stackoverflow.com/questions/34247057/how-to-read-csv-file-and-assign-to-eigen-matrix


private:
	// MatrixXd is an Eigen typdef for Matrix<double, Dynamic, Dynamic>
	MatrixXd  A,B,C; // A,B,C matrices
	MatrixXd x0;     // initial state
	MatrixXd inputSequence;  // input sequnce, dimensions: m\times  timeSamples
	MatrixXd simulatedStateSequence; //simulated state sequence, dimensions: n\times  timeSamples
	MatrixXd simulatedOutputSequence; //simulated output sequence, dimensions: r\times  timeSamples
	MatrixXd timeRowVector;           // time row vector [0,1,2,3,\ldots, timeSamples-1]
	
	int m, n, r, timeSamples; //m - input dimension, n- state dimension, r-output dimension, timeSamples- number of time samples 

};
#endif

First, we explain private member variables and objects. The code lines 68-70 are used to add the system matrices, initial state, and input sequence as private members. The private members in the code lines 71-72 are simulated state and output sequence. These sequences are computed and stored during the execution time, and they are the main result of our simulation. The code lines 73-75 are used to add the simulation time vector, matrix dimensions, and the total number of simulation steps.

Next, we explain member functions. The default constructor line 31 sets all the matrix/vector variables to 1×1 zero matrices and all the scalar variables to zero. The overloaded constructor in line 35 assigns all the private matrix, vector, and scalar variables according to the input parameters and sets the dimensions accordingly. The function in the code line 43 returns the output and state sequences as well as the simulation time vector. The function in the code line 46 is used to run the simulation. This simulation fills-in the private member variables storing simulated state and output sequences. The functions in the code lines 49, 53, and 56 are used to open and save data from and to CSV files. The implementations of these functions are based on the ideas that we explained in this post.

The implementation of these function is given in the following cpp file:

#include "SimulateSystem.h"

SimulateSystem::SimulateSystem()
{
	m = 0; n = 0; r = 0;
	A.resize(1, 1); A.setZero();
	B.resize(1, 1); B.setZero();
	C.resize(1, 1); C.setZero();
	x0.resize(1, 1); x0.setZero();
	inputSequence.resize(1, 1); inputSequence.setZero();
	simulatedStateSequence.resize(1, 1); simulatedStateSequence.setZero();
	simulatedOutputSequence.resize(1, 1); simulatedOutputSequence.setZero();
	timeRowVector.resize(1, 1); timeRowVector.setZero();
}

SimulateSystem::SimulateSystem(MatrixXd Amatrix, MatrixXd Bmatrix, MatrixXd Cmatrix, MatrixXd initialState, MatrixXd inputSequenceMatrix)
{
	A = Amatrix; B = Bmatrix; C = Cmatrix; x0 = initialState; inputSequence = inputSequenceMatrix;
	n = A.rows();
	m = B.cols();
	r = C.rows();
	timeSamples = inputSequence.cols();

	simulatedOutputSequence.resize(r, timeSamples); simulatedOutputSequence.setZero();
	simulatedStateSequence.resize(n, timeSamples);	simulatedStateSequence.setZero();
	
	timeRowVector.resize(1, timeSamples);

	for (int i = 0; i < timeSamples; i++)
	{
		timeRowVector(0, i) = i + 1;
	}

}

SimulateSystem::~SimulateSystem()
{
}

std::tuple<MatrixXd, MatrixXd, MatrixXd> SimulateSystem::getStateOuputTime()
{
	std::tuple<MatrixXd, MatrixXd, MatrixXd>   result(simulatedStateSequence, simulatedOutputSequence, timeRowVector);
	return result;
}

void SimulateSystem::saveData(std::string AFile, std::string BFile, std::string CFile, std::string x0File, std::string inputSequenceFile, std::string simulatedStateSequenceFile, std::string simulatedOutputSequenceFile) const
{
	const static IOFormat CSVFormat(FullPrecision, DontAlignCols, ", ", "\n");

	std::ofstream fileA(AFile);
	if (fileA.is_open())
	{
		fileA << A.format(CSVFormat);
		fileA.close();
	}

	std::ofstream fileB(BFile);
	if (fileB.is_open())
	{
		fileB << B.format(CSVFormat);
		fileB.close();
	}

	
	std::ofstream fileC(CFile);
	if (fileC.is_open())
	{
		fileC << C.format(CSVFormat);
		fileC.close();
	}



	std::ofstream fileX0(x0File);
	if (fileX0.is_open())
	{
		fileX0 << x0.format(CSVFormat);
		fileX0.close();
	}


	std::ofstream fileInputSequence(inputSequenceFile);
	if (fileInputSequence.is_open())
	{
		fileInputSequence << inputSequence.format(CSVFormat);
		fileInputSequence.close();
	}

	std::ofstream fileSimulatedStateSequence(simulatedStateSequenceFile);
	if (fileSimulatedStateSequence.is_open())
	{
		fileSimulatedStateSequence << simulatedStateSequence.format(CSVFormat);
		fileSimulatedStateSequence.close();
	}

	std::ofstream fileSimulatedOutputSequence(simulatedOutputSequenceFile);
	if (fileSimulatedOutputSequence.is_open())
	{
		fileSimulatedOutputSequence << simulatedOutputSequence.format(CSVFormat);
		fileSimulatedOutputSequence.close();
	}



}

MatrixXd SimulateSystem::openData(std::string fileToOpen)
{

	// the inspiration for creating this function was drawn from here (I did NOT copy and paste the code)
	// https://stackoverflow.com/questions/34247057/how-to-read-csv-file-and-assign-to-eigen-matrix
	// NOTE THAT THIS FUNCTION IS CALLED BY THE FUNCTION: SimulateSystem::openFromFile(std::string Afile, std::string Bfile, std::string Cfile, std::string x0File, std::string inputSequenceFile)
	
	// the input is the file: "fileToOpen.csv":
	// a,b,c
	// d,e,f
	// This function converts input file data into the Eigen matrix format



	// the matrix entries are stored in this variable row-wise. For example if we have the matrix:
	// M=[a b c 
	//	  d e f]
	// the entries are stored as matrixEntries=[a,b,c,d,e,f], that is the variable "matrixEntries" is a row vector
	// later on, this vector is mapped into the Eigen matrix format
	std::vector<double> matrixEntries;

	// in this object we store the data from the matrix
	std::ifstream matrixDataFile(fileToOpen);

	// this variable is used to store the row of the matrix that contains commas 
	std::string matrixRowString;

	// this variable is used to store the matrix entry;
	std::string matrixEntry;

	// this variable is used to track the number of rows
	int matrixRowNumber = 0;


	while (std::getline(matrixDataFile, matrixRowString)) // here we read a row by row of matrixDataFile and store every line into the string variable matrixRowString
	{
		std::stringstream matrixRowStringStream(matrixRowString); //convert matrixRowString that is a string to a stream variable.

		while (std::getline(matrixRowStringStream, matrixEntry,',')) // here we read pieces of the stream matrixRowStringStream until every comma, and store the resulting character into the matrixEntry
		{
			matrixEntries.push_back(std::stod(matrixEntry));   //here we convert the string to double and fill in the row vector storing all the matrix entries
			}
		matrixRowNumber++; //update the column numbers
	}

	// here we conver the vector variable into the matrix and return the resulting object, 
	// note that matrixEntries.data() is the pointer to the first memory location at which the entries of the vector matrixEntries are stored;
	return Map<Matrix<double, Dynamic, Dynamic, RowMajor>> (matrixEntries.data(), matrixRowNumber, matrixEntries.size() / matrixRowNumber);

}


void SimulateSystem::openFromFile(std::string Afile, std::string Bfile, std::string Cfile, std::string x0File, std::string inputSequenceFile)
{
	// this function acts as a constructor in some way. 
	// call this function after a default constructor is being called

	A = openData(Afile);
	B = openData(Bfile);
	C = openData(Cfile);
	x0= openData(x0File);
	inputSequence=openData(inputSequenceFile);

	n = A.rows();
	m = B.cols();
	r = C.rows();
	timeSamples = inputSequence.cols();

	simulatedOutputSequence.resize(r, timeSamples); simulatedOutputSequence.setZero();
	simulatedStateSequence.resize(n, timeSamples);	simulatedStateSequence.setZero();

	timeRowVector.resize(1, timeSamples);

	for (int i = 0; i < timeSamples; i++)
	{
		timeRowVector(0, i) = i + 1;
	}

	
}

void SimulateSystem::runSimulation()
{
	for (int j = 0; j < timeSamples; j++)
	{
		if (j == 0)
		{
			simulatedStateSequence.col(j) = x0;
			simulatedOutputSequence.col(j) = C * x0;
		}
		else
		{
			simulatedStateSequence.col(j) = A * simulatedStateSequence.col(j - 1) + B * inputSequence.col(j - 1);
			simulatedOutputSequence.col(j) = C * simulatedStateSequence.col(j);
		}
		
	}
}

Here, for brevity, we will only comment on the runSimulation() function defined in the code lines 188-204. This function is used to simulate the dynamics for the given initial condition and for the given input sequence. We use a simple for loop to simulate the dynamics that is iterated “timeSamples” times. This function demonstrates how simple it is to simulate the dynamics using the Eigen matrix library.

To plan for testing this code is as follows. First, we are going to generate random system matrices, initial state, and the input sequence in MATLAB. Then, we are going to save these objects as CSV files. Then, we are going to load these files in our C++ code, and we are going to simulate the system. Then we are going to save the simulation results in CSV files. Finally, we are going to load the simulation results in MATLAB, and we are going to compare the C++ simulation results with MATLAB simulations.

The MATLAB code for generating the matrices and simulation conditions is given below.

%csvwrite(filename,M)
clear,pack,clc

A=0.06*randn(100,100);
B=0.1*randn(100,10);
C=ones(10,100);
input=rand(10,1000);
x0=randn(100,1);
[m,sampleTime]=size(input);
[r,~]=size(C);
[n,~]=size(A)

MATLAB_state=zeros(n,sampleTime);
MATLAB_output=zeros(r,sampleTime);

for i=1:sampleTime
   if i==1
       MATLAB_state(:,i)=x0;
       MATLAB_output(:,i)=C*x0;
       
   else
       MATLAB_state(:,i)=A*MATLAB_state(:,i-1)+B*input(:,i-1);
       MATLAB_output(:,i)=C*MATLAB_state(:,i);
   end
end

plot(MATLAB_output(1,:),'r')

csvwrite("AFile.csv",A)
csvwrite("BFile.csv",B)
csvwrite("CFile.csv",C)
csvwrite("x0File.csv",x0)
csvwrite("inputFile.csv",input);

The MATLAB function for comparing the C++ simulation with the MATLAB simulation is given below.

clear,pack,clc
double x0;
double input;
double output;
double state;

% these are simulation conditions and results generated using the Eigen C++
% matrix library
output=dlmread('simulatedOutputSequenceFileOutput.csv');
state=dlmread('simulatedStateSequenceFileOutput.csv');

% these are original system matrices and conditions
A=dlmread('AFile.csv');
B=dlmread('BFile.csv');
C=dlmread('CFile.csv');
x0 = dlmread('x0File.csv');
input = dlmread('inputFile.csv');

[m,sampleTime]=size(input);
[r,~]=size(output);
[n,~]=size(A)

% these two matrices store MATLAB simulation results
MATLAB_state=zeros(n,sampleTime);
MATLAB_output=zeros(r,sampleTime);
tic
for i=1:sampleTime
   if i==1
       MATLAB_state(:,i)=x0;
       MATLAB_output(:,i)=C*x0;
       
   else
       MATLAB_state(:,i)=A*MATLAB_state(:,i-1)+B*input(:,i-1);
       MATLAB_output(:,i)=C*MATLAB_state(:,i);
   end
end
time1=toc

% compare the results
figure(1)
plot(MATLAB_output(2,:),'r')
hold on 
plot(output(2,:),'k')

figure(2)
plot(MATLAB_output(2,:)-output(2,:))

norm(MATLAB_output-output,'fro')/norm(MATLAB_output,'fro')

After running this code, we obtain the relative simulation error of $1.13 \cdot 10^{-15}$ .

admin

You might also like

Copyright Notice and License for Using Codes

ABB RobotStudio Tutorial 1: Jogging, Manipulate Views, and Motion Modes-Linear, Axis, and Reorient

How to Compute Magnitude and Phase Functions of Transfer Functions 1