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@0:d144578aa744, 2015-05-07 (annotated)

Committer:
maruta
Date:
Thu May 07 11:10:40 2015 +0000
Revision:
0:d144578aa744
Child:
1:231f20f755fe
initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
maruta 0:d144578aa744 1 #ifndef LTISYS_H_
maruta 0:d144578aa744 2 #define LTISYS_H_
maruta 0:d144578aa744 3
maruta 0:d144578aa744 4 template<int nx, int nu, int ny>
maruta 0:d144578aa744 5 class ltisys{
maruta 0:d144578aa744 6 public:
maruta 0:d144578aa744 7 ltisys(double *matA, double *matB, double *matC, double *matD);
maruta 0:d144578aa744 8 virtual ~ltisys();
maruta 0:d144578aa744 9
maruta 0:d144578aa744 10 inline void update(double dt, double *u);
maruta 0:d144578aa744 11 double x[nx], u[nu], y[ny];
maruta 0:d144578aa744 12 double A[nx][nx], B[nx][nu], C[ny][nx], D[ny][nu];
maruta 0:d144578aa744 13
maruta 0:d144578aa744 14 private:
maruta 0:d144578aa744 15 inline void matcopy(int nrow, int ncol, const double *src,
maruta 0:d144578aa744 16 double *dst);
maruta 0:d144578aa744 17 inline void mataddprod(int nrowDst, int ncolA, int ncolDst,
maruta 0:d144578aa744 18 double *matA, double *matB,
maruta 0:d144578aa744 19 double *dst);
maruta 0:d144578aa744 20 inline void matprod(int nrowDst, int ncolA, int ncolDst,
maruta 0:d144578aa744 21 double *matA, double *matB,
maruta 0:d144578aa744 22 double *dst);
maruta 0:d144578aa744 23 inline void calc_dx(double *x, double *dx);
maruta 0:d144578aa744 24 };
maruta 0:d144578aa744 25
maruta 0:d144578aa744 26 template <int nx, int nu, int ny>
maruta 0:d144578aa744 27 ltisys<nx,nu,ny>::ltisys(double *matA, double *matB, double *matC, double *matD) {
maruta 0:d144578aa744 28 matcopy(nx,nx,matA,&A[0][0]);
maruta 0:d144578aa744 29 matcopy(nx,nu,matB,&B[0][0]);
maruta 0:d144578aa744 30 matcopy(ny,nx,matC,&C[0][0]);
maruta 0:d144578aa744 31 matcopy(ny,nu,matD,&D[0][0]);
maruta 0:d144578aa744 32 for(int i=0;i<nx;i++) x[i]=0.0;
maruta 0:d144578aa744 33 for(int i=0;i<nu;i++) u[i]=0.0;
maruta 0:d144578aa744 34 for(int i=0;i<ny;i++) y[i]=0.0;
maruta 0:d144578aa744 35 }
maruta 0:d144578aa744 36
maruta 0:d144578aa744 37 template <int nx, int nu, int ny>
maruta 0:d144578aa744 38 ltisys<nx,nu,ny>::~ltisys() {
maruta 0:d144578aa744 39 }
maruta 0:d144578aa744 40
maruta 0:d144578aa744 41
maruta 0:d144578aa744 42 template <int nx, int nu, int ny>
maruta 0:d144578aa744 43 inline void ltisys<nx,nu,ny>::update(double dt, double *new_u){
maruta 0:d144578aa744 44 double k1[nx],k2[nx],k3[nx],k4[nx],xt[nx];
maruta 0:d144578aa744 45
maruta 0:d144578aa744 46 // update states by rk4 method
maruta 0:d144578aa744 47 // k1=Ax+Bu
maruta 0:d144578aa744 48 calc_dx(x,k1);
maruta 0:d144578aa744 49
maruta 0:d144578aa744 50 // k2
maruta 0:d144578aa744 51 for(int i=0;i<nx;i++){
maruta 0:d144578aa744 52 xt[i]=x[i]+dt*k1[i]/2;
maruta 0:d144578aa744 53 }
maruta 0:d144578aa744 54 calc_dx(xt,k2);
maruta 0:d144578aa744 55
maruta 0:d144578aa744 56 // k3
maruta 0:d144578aa744 57 for(int i=0;i<nx;i++){
maruta 0:d144578aa744 58 xt[i]=x[i]+dt*k2[i]/2;
maruta 0:d144578aa744 59 }
maruta 0:d144578aa744 60 calc_dx(xt,k3);
maruta 0:d144578aa744 61
maruta 0:d144578aa744 62 // k4
maruta 0:d144578aa744 63 for(int i=0;i<nx;i++){
maruta 0:d144578aa744 64 xt[i]=x[i]+dt*k3[i];
maruta 0:d144578aa744 65 }
maruta 0:d144578aa744 66 calc_dx(xt,k4);
maruta 0:d144578aa744 67
maruta 0:d144578aa744 68 for(int i=0; i<nx; i++){
maruta 0:d144578aa744 69 x[i] += dt*(k1[i]+2*k2[i]+2*k3[i]+k4[i])/6;
maruta 0:d144578aa744 70 }
maruta 0:d144578aa744 71
maruta 0:d144578aa744 72 // update I/O
maruta 0:d144578aa744 73 matprod(ny,nx,1,&C[0][0],&x[0],&y[0]);
maruta 0:d144578aa744 74 matcopy(nu,1,new_u,u);
maruta 0:d144578aa744 75 mataddprod(ny,nu,1,&D[0][0],&u[0],&y[0]);
maruta 0:d144578aa744 76 }
maruta 0:d144578aa744 77
maruta 0:d144578aa744 78 template <int nx, int nu, int ny>
maruta 0:d144578aa744 79 inline void ltisys<nx,nu,ny>::matcopy(int nrow, int ncol, const double *src,
maruta 0:d144578aa744 80 double *dst) {
maruta 0:d144578aa744 81 for (int i = 0; i < nrow*ncol; i++)
maruta 0:d144578aa744 82 dst[i] = src[i];
maruta 0:d144578aa744 83 }
maruta 0:d144578aa744 84
maruta 0:d144578aa744 85 template <int nx, int nu, int ny>
maruta 0:d144578aa744 86 inline void ltisys<nx,nu,ny>::mataddprod(int nrowDst, int ncolA, int ncolDst,
maruta 0:d144578aa744 87 double *matA, double *matB,
maruta 0:d144578aa744 88 double *dst) {
maruta 0:d144578aa744 89 for (int i = 0; i < nrowDst; i++) {
maruta 0:d144578aa744 90 for (int j = 0; j < ncolDst; j++) {
maruta 0:d144578aa744 91 double prod = 0.0;
maruta 0:d144578aa744 92 for (int k = 0; k < ncolA; k++) {
maruta 0:d144578aa744 93 prod += matA[i*ncolA+k] * matB[k*ncolDst+j];
maruta 0:d144578aa744 94 }
maruta 0:d144578aa744 95 dst[i*ncolDst+j] += prod;
maruta 0:d144578aa744 96 }
maruta 0:d144578aa744 97 }
maruta 0:d144578aa744 98 }
maruta 0:d144578aa744 99
maruta 0:d144578aa744 100 template <int nx, int nu, int ny>
maruta 0:d144578aa744 101 inline void ltisys<nx,nu,ny>::matprod(int nrowDst, int ncolA, int ncolDst,
maruta 0:d144578aa744 102 double *matA, double *matB,
maruta 0:d144578aa744 103 double *dst) {
maruta 0:d144578aa744 104 for (int i = 0; i < nrowDst; i++) {
maruta 0:d144578aa744 105 for (int j = 0; j < ncolDst; j++) {
maruta 0:d144578aa744 106 double prod = 0.0;
maruta 0:d144578aa744 107 for (int k = 0; k < ncolA; k++) {
maruta 0:d144578aa744 108 prod += matA[i*ncolA+k] * matB[k*ncolDst+j];
maruta 0:d144578aa744 109 }
maruta 0:d144578aa744 110 dst[i*ncolDst+j] = prod;
maruta 0:d144578aa744 111 }
maruta 0:d144578aa744 112 }
maruta 0:d144578aa744 113 }
maruta 0:d144578aa744 114
maruta 0:d144578aa744 115 template <int nx, int nu, int ny>
maruta 0:d144578aa744 116 inline void ltisys<nx,nu,ny>::calc_dx(double *x, double *dx) {
maruta 0:d144578aa744 117 for (int i = 0; i < nx; i++) {
maruta 0:d144578aa744 118 dx[i] = 0.0;
maruta 0:d144578aa744 119 for (int j = 0; j < nx; j++) {
maruta 0:d144578aa744 120 dx[i] += A[i][j] * x[j];
maruta 0:d144578aa744 121 }
maruta 0:d144578aa744 122 for (int j = 0; j < nu; j++) {
maruta 0:d144578aa744 123 dx[i] += B[i][j] * u[j];
maruta 0:d144578aa744 124 }
maruta 0:d144578aa744 125 }
maruta 0:d144578aa744 126 }
maruta 0:d144578aa744 127
maruta 0:d144578aa744 128 #endif /* LTISYS_H_ */