MBDyn-1.7.3
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups
id.cc
Go to the documentation of this file.
1 /* $Header: /var/cvs/mbdyn/mbdyn/mbdyn-1.0/mbdyn/elec/id.cc,v 1.24 2017/01/12 14:46:22 masarati Exp $ */
2 /*
3  * MBDyn (C) is a multibody analysis code.
4  * http://www.mbdyn.org
5  *
6  * Copyright (C) 1996-2017
7  *
8  * Pierangelo Masarati <masarati@aero.polimi.it>
9  * Paolo Mantegazza <mantegazza@aero.polimi.it>
10  *
11  * Dipartimento di Ingegneria Aerospaziale - Politecnico di Milano
12  * via La Masa, 34 - 20156 Milano, Italy
13  * http://www.aero.polimi.it
14  *
15  * Changing this copyright notice is forbidden.
16  *
17  * This program is free software; you can redistribute it and/or modify
18  * it under the terms of the GNU General Public License as published by
19  * the Free Software Foundation (version 2 of the License).
20  *
21  *
22  * This program is distributed in the hope that it will be useful,
23  * but WITHOUT ANY WARRANTY; without even the implied warranty of
24  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
25  * GNU General Public License for more details.
26  *
27  * You should have received a copy of the GNU General Public License
28  * along with this program; if not, write to the Free Software
29  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
30  */
31 
32 /* identificatore generico */
33 
34 #include "mbconfig.h" /* This goes first in every *.c,*.cc file */
35 
36 #include "id.h"
37 
38 /* Ident - begin */
39 
41  const doublereal& ldl_init)
42 : size(size), nout(nout), pdBase(NULL), ppdBase(NULL),
43 ldl(NULL), vldl(NULL), z(0), vz(0), theta(NULL), vtheta(NULL),
44 phi(NULL), y(NULL), err(NULL), pF(pf), k(0.), w(0.)
45 {
46  ASSERT(size > 0);
47  ASSERT(nout > 0);
48 
49  integer i = size*size // ldl
50  +size*nout // z
51  +size*nout // theta
52  +size // phi
53  +nout // y
54  +nout; // err
55 
56  integer iv = size // vldl
57  +nout // vz
58  +nout; // vtheta
59 
62 
63  ldl = pdBase;
64  z = ldl+size*size;
65  theta = z+size*nout;
66  phi = theta+size*nout;
67  y = phi+size;
68  err = y+nout;
69 
70  for (integer j = i; j-- > 0; ) {
71  pdBase[j] = 0.;
72  }
73 
74  vldl = ppdBase;
75  vz = vldl+size;
76  vtheta = vz+nout;
77 
78  for (integer j = size; j-- > 0; ) {
79  vldl[j] = ldl+j*size;
80  vldl[j][j] = ldl_init;
81  }
82  for (integer j = nout; j-- > 0; ) {
83  vz[j] = z+j*size;
84  vtheta[j] = theta+j*size;
85  }
86 }
87 
88 
92  SAFEDELETE(pF);
93 }
94 
96  return 1./(k*k);
97 }
98 
100  k = kk;
101  w *= kk;
102  if (w > 1.e3) {
103  for (integer i = size; i-- > 0; ) {
104  for (integer j = nout; j-- > 0; ) {
105  vz[j][i] /= w;
106  }
107  vldl[i][i] *= w;
108  }
109  w = 1.;
110  } else if (w < 1.e-20) {
111  w = 1.e-20;
112  }
113 }
114 
116  // copia il termine noto z in theta (richiesto da ldlsol_())
117  for (integer i = size*nout; i-- > 0; ) {
118  theta[i] = z[i];
119  }
120 
121  __FC_DECL__(ldlsol) (ldl, &size, theta, &size, &size, &nout);
122  return theta;
123 }
124 
125 /* in realta' si va a correggere theta senza che Ident lo sappia! */
127 {
128  for (integer i = size*nout; i-- > 0; ) {
129  theta[i] = pd[i];
130  }
131 }
132 
134  return err;
135 }
136 
137 void Ident::Update(const doublereal* pphi, const doublereal* yy) {
138  // calcolo l'errore
139  for (integer j = nout; j-- > 0; ) {
140  err[j] = yy[j];
141  for (integer i = size; i-- > 0; ) {
142  err[j] -= vtheta[j][i]*pphi[i];
143  }
144  }
145 
146  /* calcolo il nuovo forgetting factor */
147  pF->Update(err);
148  doublereal d = pF->dGet();
149  SetForgettingFactor(k = 1./sqrt(d));
150 
151  for (integer i = size; i-- > 0; ) {
152  phi[i] = pphi[i]*k;
153  }
154  for (integer j = nout; j-- > 0; ) {
155  y[j] = yy[j]*k;
156  }
157 
158  /* aggiorno la matrice fattorizzata */
159  __FC_DECL__(uldlad) (ldl, &size, &size, phi, z, &size, &nout, y);
160 }
161 
162 /* Ident - end */
163 
164 
165 /* IdentProcess - begin */
166 
167 IdentProcess::IdentProcess(unsigned int iOut, unsigned int iIn,
168  unsigned int iA, unsigned int iB)
169 : iNumOutput(iOut), iNumInput(iIn), iOrdA(iA), iOrdB(iB), pIdent(NULL)
170 {
171  ASSERT(iOut > 0);
172  ASSERT(iIn > 0);
173  // ASSERT(iOrdA >= 0); // = 0 in FIR
174  // ASSERT(iOrdB >= 0); // = 0 in AR
175 }
176 
178 {
180 }
181 
183 {
184  ASSERT(size > 0);
185  ASSERT(nout > 0);
186  ASSERT(pf != NULL);
187 
188  SAFENEWWITHCONSTRUCTOR(pIdent, Ident, Ident(size, nout, pf));
189 }
190 
192 {
193  NO_OP;
194 }
195 
196 /* IdentProcess - end */
197 
198 
199 /* IdentARXProcess - begin */
200 
201 IdentARXProcess::IdentARXProcess(unsigned int iOut, unsigned int iIn,
202  unsigned int iA, unsigned int iB,
203  ForgettingFactor* pf)
204 : IdentProcess(iOut, iIn, iA, iB),
205 size(iOut*iA+iIn*(iB+1)), pdBase(NULL), pdPhi(NULL), pdY(NULL)
206 {
207  // size e' il lato lungo di Theta
208 
209  CreateIdent(size, integer(iOut), pf);
210 
211  integer i = size // Phi
212  +iOut; // Y
213 
215 
216  pdPhi = pdBase;
217  pdY = pdPhi+size;
218 
219  for (integer j = i; j-- > 0; ) {
220  pdBase[j] = 0.;
221  }
222 }
223 
225 {
227 }
228 
230  const doublereal* pdUTmp)
231 {
232  if (iOrdA > 0) {
233  for (integer i = iNumOutput*(iOrdA-1); i-- > 0; ) {
234  pdPhi[i+iNumOutput] = pdPhi[i];
235  }
236  for (integer i = iNumOutput; i-- > 0; ) {
237  pdPhi[i] = pdY[i];
238  pdY[i] = pdYTmp[i];
239  }
240  } else {
241  // FIR
242  for (integer i = iNumOutput; i-- > 0; ) {
243  pdY[i] = pdYTmp[i];
244  }
245  }
246 
247  doublereal* pdTmp = pdPhi+iNumOutput*iOrdA;
248  // if (iOrdB == 0) // AR is implicitly satisfied
249  for (integer i = iNumInput*iOrdB; i-- > 0; ) {
250  pdTmp[i+iNumInput] = pdTmp[i];
251  }
252  for (integer i = iNumInput; i-- > 0; ) {
253  pdTmp[i] = pdUTmp[i];
254  }
255 
256  pIdent->Update(pdPhi, pdY);
257 }
258 
260 {
261  doublereal* p = pIdent->pdGetTheta();
262  for (integer i = iNumOutput*size; i-- > 0; ) {
263  pdTheta[i] = p[i];
264  }
265 }
266 
267 /* IdentARXProcess - end */
268 
269 
270 /* IdentARMAXProcess - begin */
271 
272 IdentARMAXProcess::IdentARMAXProcess(unsigned int iOut, unsigned int iIn,
273  unsigned int iA, unsigned int iB,
274  ForgettingFactor* pf)
275 : IdentProcess(iOut, iIn, iA, iB),
276 size(2*iOut*iA+iIn*(iB+1)), pdBase(NULL), pdPhi(NULL), pdY(NULL), pdErr(NULL)
277 {
278  // size e' il lato lungo di Theta.
279 
280  CreateIdent(size, integer(iOut), pf);
281 
282  integer i = size // Phi
283  +iOut // Y
284  +iOut; // Err
285 
287 
288  pdPhi = pdBase;
289  pdY = pdPhi+size;
290  pdErr = pdY+iOut;
291 
292 
293  for (integer j = i; j-- > 0; ) {
294  pdBase[j] = 0.;
295  }
296 }
297 
300 }
301 
302 
303 // questa operazione deve essere svolta ad ogni passo (del controllo)
305  const doublereal* pdUTmp)
306 {
307  // qui iniziano gli errori
309 
310  // sposta di un passo (iNumOutput) le uscite e gli errori
311  for (integer i = iNumOutput*(iOrdA-1); i-- > 0; ) {
312  pdPhi[i+iNumOutput] = pdPhi[i]; // Y vecchi
313  pdTmp[i+iNumOutput] = pdTmp[i]; // Err vecchi
314  }
315  // aggiunge in testa le uscite e gli errori al passo precedente
316  for (integer i = iNumOutput; i-- > 0; ) {
317  pdPhi[i] = pdY[i]; // Y all'ultima iterazione
318  pdY[i] = pdYTmp[i]; // Y nuovo
319  pdTmp[i] = pdErr[i]; // Err all'ultima iterazione
320  }
321 
322  // qui iniziano gli ingressi
323  pdTmp = pdPhi+iNumOutput*iOrdA;
324  // sposta di un passo (iNumInput) gli ingressi
325  // if (iOrdB == 0) // ARMA is implicitly satisfied
326  for (integer i = iNumInput*iOrdB; i-- > 0; ) {
327  pdTmp[i+iNumInput] = pdTmp[i];
328  }
329  // aggiunge in testa gli ingressi al passo corrente
330  for (integer i = iNumInput; i-- > 0; ) {
331  pdTmp[i] = pdUTmp[i];
332  }
333 
334  // fa aggiornare la stima di Theta
335  pIdent->Update(pdPhi, pdY);
336 
337  // err al passo corrente e' stato calcolato da Ident; lo ricopia
338  doublereal* pd = pIdent->pdGetErr();
339  for (integer i = iNumOutput; i-- > 0; ) {
340  pdErr[i] = pd[i];
341  }
342 }
343 
344 
345 // questa operazione puo' essere svolta in modo asincrono, ovvero ogni tanto
347 {
348  // fa calcolare il nuovo theta
349  doublereal* p = pIdent->pdGetTheta();
350 
351  // se e' il caso, corregge la parte di MA di Theta
352  if (fCheckMA(p)) {
353  // se la parte MA di theta e' stata corretta, e' stata aggiornata in loco
354  NO_OP;
355  }
356 
357  // restituisce il nuovo theta
358  for (integer i = iNumOutput*size; i-- > 0; ) {
359  pdTheta[i] = p[i];
360  }
361 }
362 
364 {
365  for (integer i = iNumOutput; i-- > 0; ) {
366  pdE[i] = pdErr[i];
367  }
368 }
369 
371 {
372  // fa la correzione di Theta in base agli autovalori ed autovettori
373  return flag(0);
374 }
375 
376 /* IdentARMAXProcess - end */
377 
virtual void Update(const doublereal *pErr=NULL)=0
doublereal * pdGetTheta(void)
Definition: id.cc:115
virtual void GetTheta(doublereal *pdTheta)
Definition: id.cc:259
doublereal * pdBase
Definition: id.h:49
IdentARXProcess(unsigned int iOut, unsigned int iIn, unsigned int iA, unsigned int iB, ForgettingFactor *pf)
Definition: id.cc:201
long int flag
Definition: mbdyn.h:43
flag fCheckMA(doublereal *pdTheta)
Definition: id.cc:370
Definition: id.h:44
doublereal ** vz
Definition: id.h:56
integer size
Definition: id.h:46
void SetForgettingFactor(const doublereal &kk)
Definition: id.cc:99
integer nout
Definition: id.h:47
unsigned int iOrdA
Definition: id.h:92
void CreateIdent(integer size, integer nout, ForgettingFactor *pf)
Definition: id.cc:182
#define SAFEDELETEARR(pnt)
Definition: mynewmem.h:713
virtual ~IdentProcess(void)
Definition: id.cc:177
virtual ~IdentARXProcess(void)
Definition: id.cc:224
doublereal * pdErr
Definition: id.h:163
doublereal * z
Definition: id.h:55
doublereal * pdBase
Definition: id.h:159
#define NO_OP
Definition: myassert.h:74
doublereal * pdGetErr(void)
Definition: id.cc:133
void Update(const doublereal *pphi, const doublereal *yy)
Definition: id.cc:137
IdentProcess(unsigned int iOut, unsigned int iIn, unsigned int iA, unsigned int iB)
Definition: id.cc:167
virtual void Update(const doublereal *pdYTmp, const doublereal *pdUTmp)
Definition: id.cc:229
integer size
Definition: id.h:127
integer size
Definition: id.h:156
ForgettingFactor * pF
Definition: id.h:65
doublereal * phi
Definition: id.h:61
doublereal dGetForgettingFactor(void) const
Definition: id.cc:95
doublereal * pdPhi
Definition: id.h:161
int ldlsol(doublereal *ldl, integer *nrdldl, doublereal *b, integer *nrdb, integer *n, integer *nvet)
IdentARMAXProcess(unsigned int iOut, unsigned int iIn, unsigned int iA, unsigned int iB, ForgettingFactor *pf)
Definition: id.cc:272
doublereal * pdY
Definition: id.h:162
doublereal * theta
Definition: id.h:58
virtual ~Ident(void)
Definition: id.cc:89
void GetErr(doublereal *pdE)
Definition: id.cc:363
virtual doublereal dGet(void) const =0
doublereal w
Definition: id.h:67
#define ASSERT(expression)
Definition: colamd.c:977
unsigned int iNumOutput
Definition: id.h:90
GradientExpression< UnaryExpr< FuncSqrt, Expr > > sqrt(const GradientExpression< Expr > &u)
Definition: gradient.h:2974
doublereal * pdBase
Definition: id.h:130
doublereal * err
Definition: id.h:63
#define SAFENEWWITHCONSTRUCTOR(pnt, item, constructor)
Definition: mynewmem.h:698
doublereal ** ppdBase
Definition: id.h:50
doublereal * ldl
Definition: id.h:52
Ident * pIdent
Definition: id.h:95
doublereal ** vtheta
Definition: id.h:59
virtual void GetErr(doublereal *pdE)
Definition: id.cc:191
void UpdateTheta(const doublereal *pd)
Definition: id.cc:126
doublereal * y
Definition: id.h:62
#define SAFENEWARR(pnt, item, sz)
Definition: mynewmem.h:701
unsigned int iNumInput
Definition: id.h:91
doublereal ** vldl
Definition: id.h:53
int uldlad(doublereal *ldl, integer *nrdldl, integer *n, doublereal *x, doublereal *z, integer *nrdz, integer *nz, doublereal *y)
virtual void GetTheta(doublereal *pdTheta)
Definition: id.cc:346
double doublereal
Definition: colamd.c:52
virtual ~IdentARMAXProcess(void)
Definition: id.cc:298
long int integer
Definition: colamd.c:51
doublereal * pdPhi
Definition: id.h:132
Ident(integer size, integer nout, ForgettingFactor *pf, const doublereal &ldl_init=LDL_INIT)
Definition: id.cc:40
virtual void Update(const doublereal *pdYTmp, const doublereal *pdUTmp)
Definition: id.cc:304
doublereal * pdY
Definition: id.h:133
#define SAFEDELETE(pnt)
Definition: mynewmem.h:710
doublereal k
Definition: id.h:66
unsigned int iOrdB
Definition: id.h:93