Ichiro Maruta / ltisys

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   ltisys_test

ltisys.h@3:e093e92b7039, 2015-05-08 (annotated)

Committer:
maruta
Date:
Fri May 08 17:23:03 2015 +0000
Revision:
3:e093e92b7039
Parent:
2:ea36561fdc00
Child:
4:d459418c3440
fixed license link

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maruta 2:ea36561fdc00 1 /* ltisys
maruta 2:ea36561fdc00 2 * implementation of continuous-time time-invariant linear systems
maruta 2:ea36561fdc00 3 * Copyright (c) 2015 Ichiro Maruta
maruta 3:e093e92b7039 4 * Released under the MIT License: http://opensource.org/licenses/mit-license.php
maruta 2:ea36561fdc00 5 */
maruta 2:ea36561fdc00 6
maruta 0:d144578aa744 7 #ifndef LTISYS_H_
maruta 0:d144578aa744 8 #define LTISYS_H_
maruta 0:d144578aa744 9
maruta 1:231f20f755fe 10 /** ltisys class
maruta 1:231f20f755fe 11 * Implementation of continuous-time linear time-invariant system by RK4
maruta 1:231f20f755fe 12 *
maruta 1:231f20f755fe 13 * @tparam nx number of states
maruta 1:231f20f755fe 14 * @tparam nu number of inputs
maruta 1:231f20f755fe 15 * @tparam ny number of outputs
maruta 1:231f20f755fe 16 */
maruta 0:d144578aa744 17 template<int nx, int nu, int ny>
maruta 0:d144578aa744 18 class ltisys{
maruta 0:d144578aa744 19 public:
maruta 1:231f20f755fe 20 /** Create ltisys instance
maruta 1:231f20f755fe 21 * implements
maruta 1:231f20f755fe 22 * dx/dt = Ax +Bu,
maruta 1:231f20f755fe 23 * y = Cx + Du
maruta 1:231f20f755fe 24 * @param matA A-matrix in continuous-time linear state-space representation
maruta 1:231f20f755fe 25 * @param matB B-matrix in continuous-time linear state-space representation
maruta 1:231f20f755fe 26 * @param matC C-matrix in continuous-time linear state-space representation
maruta 1:231f20f755fe 27 * @param matD D-matrix in continuous-time linear state-space representation
maruta 1:231f20f755fe 28 *
maruta 1:231f20f755fe 29 * @note All matrices are row major in this library. Different from MATLAB/FORTRAN.
maruta 1:231f20f755fe 30 */
maruta 0:d144578aa744 31 ltisys(double *matA, double *matB, double *matC, double *matD);
maruta 0:d144578aa744 32 virtual ~ltisys();
maruta 0:d144578aa744 33
maruta 1:231f20f755fe 34 /** Update states and outputs
maruta 1:231f20f755fe 35 * updates internal states and outputs of the system
maruta 1:231f20f755fe 36 * calculation is done by 4th order Runge-Kutta method
maruta 1:231f20f755fe 37 * @param dt time elapsed from the last update
maruta 1:231f20f755fe 38 * @param u new input
maruta 1:231f20f755fe 39 */
maruta 0:d144578aa744 40 inline void update(double dt, double *u);
maruta 1:231f20f755fe 41
maruta 1:231f20f755fe 42 /// system states
maruta 1:231f20f755fe 43 double x[nx];
maruta 1:231f20f755fe 44
maruta 1:231f20f755fe 45 /// applied inputs
maruta 1:231f20f755fe 46 double u[nu];
maruta 1:231f20f755fe 47
maruta 1:231f20f755fe 48 /// system outputs
maruta 1:231f20f755fe 49 double y[ny];
maruta 1:231f20f755fe 50
maruta 0:d144578aa744 51 double A[nx][nx], B[nx][nu], C[ny][nx], D[ny][nu];
maruta 0:d144578aa744 52
maruta 0:d144578aa744 53 private:
maruta 0:d144578aa744 54 inline void matcopy(int nrow, int ncol, const double *src,
maruta 0:d144578aa744 55 double *dst);
maruta 0:d144578aa744 56 inline void mataddprod(int nrowDst, int ncolA, int ncolDst,
maruta 0:d144578aa744 57 double *matA, double *matB,
maruta 0:d144578aa744 58 double *dst);
maruta 0:d144578aa744 59 inline void matprod(int nrowDst, int ncolA, int ncolDst,
maruta 0:d144578aa744 60 double *matA, double *matB,
maruta 0:d144578aa744 61 double *dst);
maruta 0:d144578aa744 62 inline void calc_dx(double *x, double *dx);
maruta 0:d144578aa744 63 };
maruta 0:d144578aa744 64
maruta 0:d144578aa744 65 template <int nx, int nu, int ny>
maruta 0:d144578aa744 66 ltisys<nx,nu,ny>::ltisys(double *matA, double *matB, double *matC, double *matD) {
maruta 0:d144578aa744 67 matcopy(nx,nx,matA,&A[0][0]);
maruta 0:d144578aa744 68 matcopy(nx,nu,matB,&B[0][0]);
maruta 0:d144578aa744 69 matcopy(ny,nx,matC,&C[0][0]);
maruta 0:d144578aa744 70 matcopy(ny,nu,matD,&D[0][0]);
maruta 0:d144578aa744 71 for(int i=0;i<nx;i++) x[i]=0.0;
maruta 0:d144578aa744 72 for(int i=0;i<nu;i++) u[i]=0.0;
maruta 0:d144578aa744 73 for(int i=0;i<ny;i++) y[i]=0.0;
maruta 0:d144578aa744 74 }
maruta 0:d144578aa744 75
maruta 0:d144578aa744 76 template <int nx, int nu, int ny>
maruta 0:d144578aa744 77 ltisys<nx,nu,ny>::~ltisys() {
maruta 0:d144578aa744 78 }
maruta 0:d144578aa744 79
maruta 0:d144578aa744 80
maruta 0:d144578aa744 81 template <int nx, int nu, int ny>
maruta 0:d144578aa744 82 inline void ltisys<nx,nu,ny>::update(double dt, double *new_u){
maruta 0:d144578aa744 83 double k1[nx],k2[nx],k3[nx],k4[nx],xt[nx];
maruta 0:d144578aa744 84
maruta 0:d144578aa744 85 // update states by rk4 method
maruta 0:d144578aa744 86 // k1=Ax+Bu
maruta 0:d144578aa744 87 calc_dx(x,k1);
maruta 0:d144578aa744 88
maruta 0:d144578aa744 89 // k2
maruta 0:d144578aa744 90 for(int i=0;i<nx;i++){
maruta 0:d144578aa744 91 xt[i]=x[i]+dt*k1[i]/2;
maruta 0:d144578aa744 92 }
maruta 0:d144578aa744 93 calc_dx(xt,k2);
maruta 0:d144578aa744 94
maruta 0:d144578aa744 95 // k3
maruta 0:d144578aa744 96 for(int i=0;i<nx;i++){
maruta 0:d144578aa744 97 xt[i]=x[i]+dt*k2[i]/2;
maruta 0:d144578aa744 98 }
maruta 0:d144578aa744 99 calc_dx(xt,k3);
maruta 0:d144578aa744 100
maruta 0:d144578aa744 101 // k4
maruta 0:d144578aa744 102 for(int i=0;i<nx;i++){
maruta 0:d144578aa744 103 xt[i]=x[i]+dt*k3[i];
maruta 0:d144578aa744 104 }
maruta 0:d144578aa744 105 calc_dx(xt,k4);
maruta 0:d144578aa744 106
maruta 0:d144578aa744 107 for(int i=0; i<nx; i++){
maruta 0:d144578aa744 108 x[i] += dt*(k1[i]+2*k2[i]+2*k3[i]+k4[i])/6;
maruta 0:d144578aa744 109 }
maruta 0:d144578aa744 110
maruta 0:d144578aa744 111 // update I/O
maruta 0:d144578aa744 112 matprod(ny,nx,1,&C[0][0],&x[0],&y[0]);
maruta 0:d144578aa744 113 matcopy(nu,1,new_u,u);
maruta 0:d144578aa744 114 mataddprod(ny,nu,1,&D[0][0],&u[0],&y[0]);
maruta 0:d144578aa744 115 }
maruta 0:d144578aa744 116
maruta 0:d144578aa744 117 template <int nx, int nu, int ny>
maruta 0:d144578aa744 118 inline void ltisys<nx,nu,ny>::matcopy(int nrow, int ncol, const double *src,
maruta 0:d144578aa744 119 double *dst) {
maruta 0:d144578aa744 120 for (int i = 0; i < nrow*ncol; i++)
maruta 0:d144578aa744 121 dst[i] = src[i];
maruta 0:d144578aa744 122 }
maruta 0:d144578aa744 123
maruta 0:d144578aa744 124 template <int nx, int nu, int ny>
maruta 0:d144578aa744 125 inline void ltisys<nx,nu,ny>::mataddprod(int nrowDst, int ncolA, int ncolDst,
maruta 0:d144578aa744 126 double *matA, double *matB,
maruta 0:d144578aa744 127 double *dst) {
maruta 0:d144578aa744 128 for (int i = 0; i < nrowDst; i++) {
maruta 0:d144578aa744 129 for (int j = 0; j < ncolDst; j++) {
maruta 0:d144578aa744 130 double prod = 0.0;
maruta 0:d144578aa744 131 for (int k = 0; k < ncolA; k++) {
maruta 0:d144578aa744 132 prod += matA[i*ncolA+k] * matB[k*ncolDst+j];
maruta 0:d144578aa744 133 }
maruta 0:d144578aa744 134 dst[i*ncolDst+j] += prod;
maruta 0:d144578aa744 135 }
maruta 0:d144578aa744 136 }
maruta 0:d144578aa744 137 }
maruta 0:d144578aa744 138
maruta 0:d144578aa744 139 template <int nx, int nu, int ny>
maruta 0:d144578aa744 140 inline void ltisys<nx,nu,ny>::matprod(int nrowDst, int ncolA, int ncolDst,
maruta 0:d144578aa744 141 double *matA, double *matB,
maruta 0:d144578aa744 142 double *dst) {
maruta 0:d144578aa744 143 for (int i = 0; i < nrowDst; i++) {
maruta 0:d144578aa744 144 for (int j = 0; j < ncolDst; j++) {
maruta 0:d144578aa744 145 double prod = 0.0;
maruta 0:d144578aa744 146 for (int k = 0; k < ncolA; k++) {
maruta 0:d144578aa744 147 prod += matA[i*ncolA+k] * matB[k*ncolDst+j];
maruta 0:d144578aa744 148 }
maruta 0:d144578aa744 149 dst[i*ncolDst+j] = prod;
maruta 0:d144578aa744 150 }
maruta 0:d144578aa744 151 }
maruta 0:d144578aa744 152 }
maruta 0:d144578aa744 153
maruta 0:d144578aa744 154 template <int nx, int nu, int ny>
maruta 0:d144578aa744 155 inline void ltisys<nx,nu,ny>::calc_dx(double *x, double *dx) {
maruta 0:d144578aa744 156 for (int i = 0; i < nx; i++) {
maruta 0:d144578aa744 157 dx[i] = 0.0;
maruta 0:d144578aa744 158 for (int j = 0; j < nx; j++) {
maruta 0:d144578aa744 159 dx[i] += A[i][j] * x[j];
maruta 0:d144578aa744 160 }
maruta 0:d144578aa744 161 for (int j = 0; j < nu; j++) {
maruta 0:d144578aa744 162 dx[i] += B[i][j] * u[j];
maruta 0:d144578aa744 163 }
maruta 0:d144578aa744 164 }
maruta 0:d144578aa744 165 }
maruta 0:d144578aa744 166
maruta 0:d144578aa744 167 #endif /* LTISYS_H_ */