00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 double**
allocateMatrix(
int m,
int n){
00015
double** mat=(
double**)malloc(m*
sizeof(
double*));
00016
for(
int i=0; i < m; i++){
00017 mat[i]=(
double*)malloc(n*
sizeof(
double));
00018 }
00019
return mat;
00020 }
00021
00022 void freeMatrix(
double** mat,
int m){
00023
for(
int i=0; i < m; i++){
00024 free(mat[i]);
00025 }
00026 free(mat);
00027 }
00028
00029
00030
public:
00031
00032
00033 virtual constparamkey
getName()
const {
00034
return name;
00035 }
00036
00037
00038 virtual void init(
int sensornumber,
int motornumber){
00039
00040 }
00041
00042
00043 DInvert3ChannelController(
int numberchannels,
int buffersize){
00044
00045 eps=0.7;
00046 rho=0.0;
00047 s4delay=1;
00048 s4avg=1;
00049 delta=0.01;
00050 factor_a=1.0;
00051
t=0;
00052 m=0.0;
00053
00054 NUMBER_CHANNELS=numberchannels;
00055 BUFFER_SIZE=buffersize;
00056
00057 A =
allocateMatrix(NUMBER_CHANNELS, NUMBER_CHANNELS);
00058 C =
allocateMatrix(NUMBER_CHANNELS, NUMBER_CHANNELS);
00059 h = (
double*)malloc(NUMBER_CHANNELS*
sizeof(
double));
00060
00061 x_buffer =
allocateMatrix(BUFFER_SIZE, NUMBER_CHANNELS);
00062 y_buffer =
allocateMatrix(BUFFER_SIZE, NUMBER_CHANNELS);
00063
00064 xsi4E = (
double*)malloc(NUMBER_CHANNELS*
sizeof(
double));
00065 xsi4Model = (
double*)malloc(NUMBER_CHANNELS*
sizeof(
double));
00066
00067
00068 x_smooth = (
double*)malloc(NUMBER_CHANNELS*
sizeof(
double));
00069 x_effective = (
double*)malloc(NUMBER_CHANNELS*
sizeof(
double));
00070 y_effective = (
double*)malloc(NUMBER_CHANNELS*
sizeof(
double));
00071 Q_buf1 =
allocateMatrix(NUMBER_CHANNELS, NUMBER_CHANNELS);
00072 Q_buf2 =
allocateMatrix(NUMBER_CHANNELS, NUMBER_CHANNELS);
00073 L =
allocateMatrix(NUMBER_CHANNELS, NUMBER_CHANNELS);
00074 z = (
double*)malloc(NUMBER_CHANNELS*
sizeof(
double));
00075
00076
00077
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00078 {
00079 h[i] = 0.0;
00080
for (
int j = 0; j < NUMBER_CHANNELS; j++)
00081 {
00082
if (i == j)
00083 {
00084 A[i][j] = (rand()/10.0)/RAND_MAX;
00085 C[i][j] = (rand()/10.0)/RAND_MAX;
00086 }
00087
else
00088 {
00089 A[i][j] = (rand()/10.0)/RAND_MAX;
00090 C[i][j] = (rand()/10.0)/RAND_MAX;
00091 }
00092 }
00093 }
00094
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00095 {
00096
for (
int k = 0; k < BUFFER_SIZE; k++)
00097 {
00098 x_buffer[k][i] = 0;
00099 y_buffer[k][i] = 0;
00100 }
00101 }
00102
00103
Configurable::insertCVSInfo(name,
"$RCSfile: dinvert3channelcontroller.hpp,v $",
00104
"$Revision: 1.5 $");
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115 };
00116
00117
00118 virtual void step(
const sensor* x_,
int sensornumber,
motor* y_,
int motornumber){
00119
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00120 {
00121 x_smooth[i] = 0.0;
00122 x_effective[i] = 0.0;
00123 y_effective[i] = 0.0;
00124 }
00125
00126
00127
putInBuffer(x_buffer, x_);
00128
00129
00130
calculateSmoothValues(x_buffer, s4avg, x_smooth);
00131
00132
00133
calculateControllerValues(x_smooth, y_);
00134
00135
00136
putInBuffer(y_buffer, y_);
00137
00138
00139
calculateDelayedValues(x_buffer, s4delay, x_effective);
00140
calculateDelayedValues(y_buffer, s4delay, y_effective);
00141
00142
00143
learn(x_, x_effective, y_effective);
00144
00145
00146
learnModel(x_, y_effective);
00147
00148
00149
t++;
00150 };
00151
00152
00153
00154 virtual void stepNoLearning(
const sensor* x_,
int sensornumber,
motor* y_,
int motornumber){
00155
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00156 {
00157 x_smooth[i] = 0.0;
00158 }
00159
00160
00161
00162
putInBuffer(x_buffer, x_);
00163
00164
00165
calculateSmoothValues(x_buffer, s4avg, x_smooth);
00166
00167
00168
calculateControllerValues(x_smooth, y_);
00169
00170
00171
putInBuffer(y_buffer, y_);
00172
00173
00174
t++;
00175 };
00176
00177 virtual int getInternalParamNames(paramkey*& keylist){
00178
return 0;
00179 }
00180
00181 virtual int getInternalParams(paramval* vallist,
int length){
00182
int len = 0;
00183
return len;
00184 }
00185
00186
protected:
00187 virtual void inverseMatrix(
double** Q,
double** Q_1){
00188
00189
00190
00191 assert(NUMBER_CHANNELS<4);
00192
if (NUMBER_CHANNELS==2){
00193
00194
double det = Q[0][0] * Q[1][1] - Q[0][1] * Q[1][0];
00195 Q_1[0][0] = Q[1][1] / det;
00196 Q_1[1][1] = Q[0][0] / det;
00197 Q_1[0][1] = -Q[0][1] / det;
00198 Q_1[1][0] = -Q[1][0] / det;
00199
00200 }
00201
00202
00203
if (NUMBER_CHANNELS==3){
00204
00205
double detQ=0 ;
00206
double** Q_adjoint=Q_buf1;
00207
00208
00209 Q_adjoint[0][0]=Q[1][1]*Q[2][2]-Q[1][2]*Q[2][1] ;
00210 Q_adjoint[0][1]=(Q[1][2]*Q[2][0]-Q[1][0]*Q[2][2]) ;
00211 Q_adjoint[0][2]=Q[1][0]*Q[2][1]-Q[1][1]*Q[2][0] ;
00212 Q_adjoint[1][0]=(Q[2][1]*Q[0][2]-Q[0][1]*Q[2][2]) ;
00213 Q_adjoint[1][1]=Q[0][0]*Q[2][2]-Q[0][2]*Q[2][0] ;
00214 Q_adjoint[1][2]=(Q[0][1]*Q[2][0]-Q[0][0]*Q[2][1]) ;
00215 Q_adjoint[2][0]=Q[0][1]*Q[1][2]-Q[1][1]*Q[0][2] ;
00216 Q_adjoint[2][1]=(Q[1][0]*Q[0][2]-Q[0][0]*Q[1][2]) ;
00217 Q_adjoint[2][2]=Q[0][0]*Q[1][1]-Q[0][1]*Q[1][0] ;
00218 detQ=Q[0][0]*Q_adjoint[0][0]+Q[0][1]*Q_adjoint[0][1]+Q[0][2]*Q_adjoint[0][2] ;
00219
for(
int i=0; i<NUMBER_CHANNELS; i++){
00220
for(
int j=0; j<NUMBER_CHANNELS; j++) {
00221 Q_1[i][j]=(Q_adjoint[j][i])/detQ ;
00222 }
00223 }
00224 }
00225 };
00226
00227
00228 virtual double calculateE(
const double *x_,
const double *x_delay,
const double *y_delay) {
00229
double** Q = Q_buf1;
00230
double** Q_1= Q_buf2;
00231
00232
00233
00234
00235
00236
00237
00238
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00239 {
00240 z[i] = h[i];
00241
for (
int j = 0; j < NUMBER_CHANNELS; j++)
00242 {
00243 z[i] += C[i][j] * x_delay[j];
00244 }
00245 }
00246
00247
00248
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00249 {
00250
for (
int j = 0; j < NUMBER_CHANNELS; j++)
00251 {
00252 L[i][j] = 0.0;
00253
for (
int k = 0; k < NUMBER_CHANNELS; k++)
00254 {
00255 L[i][j] += A[i][k] *
g_s(z[k]) * C[k][j];
00256 }
00257 }
00258 }
00259
00260
00261
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00262 {
00263
for (
int j = 0; j < NUMBER_CHANNELS; j++)
00264 {
00265 Q[i][j] = 0.0;
00266
for (
int k = 0; k < NUMBER_CHANNELS; k++)
00267 {
00268 Q[i][j] += L[i][k] * L[j][k];
00269 }
00270
if (i == j)
00271 Q[i][j] += rho / NUMBER_CHANNELS;
00272 }
00273 }
00274
00275
00276
inverseMatrix(Q, Q_1);
00277
00278
00279
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00280 {
00281 xsi4E[i] = x_[i];
00282
for (
int j = 0; j < NUMBER_CHANNELS; j++)
00283 {
00284 xsi4E[i] -= A[i][j] * y_delay[j];
00285
00286
00287 }
00288 }
00289
double E = 0;
00290
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00291 {
00292
for (
int j = 0; j < NUMBER_CHANNELS; j++)
00293 {
00294 E += xsi4E[i] * Q_1[i][j] * xsi4E[j];
00295 }
00296 }
00297
00298
double E_s=0;
00299
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00300 {
00301
for (
int j = 0; j < NUMBER_CHANNELS; j++)
00302 {
00303 E_s += (A[i][j]*
g(z[j]) - x_[i]) * (A[i][j]*
g(z[j]) - x_[i]);
00304 }
00305 }
00306
00307 E=(1-m)*E+ m*E_s;
00308
return (E);
00309
00310 };
00311
00312
00313 virtual void learn(
const double *x_,
const double *x_delay,
const double *y_delay)
00314 {
00315
00316
double** C_update=
allocateMatrix(NUMBER_CHANNELS,NUMBER_CHANNELS);
00317
double* h_update=(
double*)malloc(NUMBER_CHANNELS*
sizeof(
double));
00318
00319
double E_0 =
calculateE(x_,x_delay, y_delay);
00320
00321
00322
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00323 {
00324 h[i] += delta;
00325 h_update[i] = -eps * (
calculateE(x_,x_delay, y_delay) - E_0) / delta;
00326 h[i] -= delta;
00327
for (
int j = 0; j < NUMBER_CHANNELS; j++)
00328 {
00329 C[i][j] += delta;
00330 C_update[i][j] = -eps * (
calculateE(x_,x_delay, y_delay) - E_0) / delta;
00331 C[i][j] -= delta;
00332
00333
00334
00335 }
00336 }
00337
00338
00339
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00340 {
00341 h[i] += squash(h_update[i]);
00342
for (
int j = 0; j < NUMBER_CHANNELS; j++)
00343 {
00344 C[i][j] += squash(C_update[i][j]);
00345
00346 }
00347 }
00348
freeMatrix(C_update, NUMBER_CHANNELS);
00349 free(h_update);
00350 };
00351
00352
00353 virtual void learnModel(
const double *x_actual,
double *y_effective){
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
for(
int i=0; i<NUMBER_CHANNELS; i++){
00364 xsi4Model[i]=x_actual[i];
00365
for(
int j=0; j<NUMBER_CHANNELS; j++){
00366 xsi4Model[i]-= A[i][j]*y_effective[j];
00367 }
00368 }
00369
00370
for(
int i=0; i<NUMBER_CHANNELS; i++){
00371
for (
int j=0; j<NUMBER_CHANNELS; j++){
00372 A[i][j]+=squash( (factor_a*eps*0.2) *xsi4Model[i] * y_effective[j]) ;
00373 }
00374 }
00375 };
00376
00377
00378
00379
00380
00381
00382 virtual void calculateDelayedValues(
double** source, paramval number_steps_of_delay_,
double *target)
00383 {
00384
00385 assert ((
int)number_steps_of_delay_ < BUFFER_SIZE);
00386
00387
00388
00389
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00390 {
00391 target[i] = source[(
t - (
int)number_steps_of_delay_ + BUFFER_SIZE) % BUFFER_SIZE][i];
00392 }
00393 };
00394
00395 virtual void calculateSmoothValues(
double** source, paramval number_steps_for_averaging_,
double *target)
00396 {
00397
00398 assert ((
int)number_steps_for_averaging_ <= BUFFER_SIZE);
00399
00400
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00401 {
00402 target[i] = 0.0;
00403
for (
int k = 0; k < (
int)number_steps_for_averaging_; k++)
00404 {
00405 target[i] += source[(
t - k + BUFFER_SIZE) % BUFFER_SIZE][i]/ (
double) (number_steps_for_averaging_);
00406 }
00407 }
00408 };
00409
00410
00411
00412
00413 virtual void calculateControllerValues(
double *x_smooth,
double *y)
00414 {
00415
double z[NUMBER_CHANNELS];
00416
00417
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00418 {
00419 z[i] = h[i];
00420
for (
int j = 0; j < NUMBER_CHANNELS; j++)
00421 {
00422 z[i] += C[i][j] * x_smooth[j];
00423 }
00424 y[i] =
g(z[i]);
00425 }
00426 };
00427
00428
00429
00430 virtual void putInBuffer(
double** buffer,
const double *values){
00431
for (
int i = 0; i < NUMBER_CHANNELS; i++)
00432 {
00433 buffer[(
t+BUFFER_SIZE)% BUFFER_SIZE][i] = values[i];
00434 }
00435 };
00436