som.cpp

VC++实现神经网络SOM（自组织特征映射网）

/******************************************************************************

                      ===================
        Network:      Self-Organizing Map
                      ===================

        Application:  Control
                      Pole Balancing Problem

        Author:       Karsten Kutza
        Date:         6.6.96

        Reference:    T. Kohonen	
                      Self-Organized Formation
                      of Topologically Correct Feature Maps
                      Biological Cybernetics, 43, pp. 59-69, 1982

 ******************************************************************************/




/******************************************************************************
                            D E C L A R A T I O N S
 ******************************************************************************/


#include <stdlib.h>
#include <stdio.h>
#include <math.h>


typedef int           BOOL;
typedef int           INT;
typedef double        REAL;

#define FALSE         0
#define TRUE          1
#define NOT           !
#define AND           &&
#define OR            ||

#define MIN_REAL      -HUGE_VAL
#define MAX_REAL      +HUGE_VAL
#define MIN(x,y)      ((x)<(y) ? (x) : (y))
#define MAX(x,y)      ((x)>(y) ? (x) : (y))

#define PI            (2*asin((double)(1)))
#define sqr(x)        ((x)*(x))


typedef struct {                     /* A LAYER OF A NET:                     */
        INT           Units;         /* - number of units in this layer       */
        REAL*         Output;        /* - output of ith unit                  */
        REAL**        Weight;        /* - connection weights to ith unit      */
        REAL*         StepSize;      /* - size of search steps of ith unit    */
        REAL*         dScoreMean;    /* - mean score delta of ith unit        */
} LAYER;

typedef struct {                     /* A NET:                                */
        LAYER*        InputLayer;    /* - input layer                         */
        LAYER*        KohonenLayer;  /* - Kohonen layer                       */
        LAYER*        OutputLayer;   /* - output layer                        */
        INT           Winner;        /* - last winner in Kohonen layer        */
        REAL          Alpha;         /* - learning rate for Kohonen layer     */
        REAL          Alpha_;        /* - learning rate for output layer      */
        REAL          Alpha__;       /* - learning rate for step sizes        */
        REAL          Gamma;         /* - smoothing factor for score deltas   */
        REAL          Sigma;         /* - parameter for width of neighborhood */
} NET;


/******************************************************************************
        R A N D O M S   D R A W N   F R O M   D I S T R I B U T I O N S
 ******************************************************************************/


void InitializeRandoms()
{
  srand(4715);
}      


REAL RandomEqualREAL(REAL Low, REAL High)
{
  return ((REAL) rand() / RAND_MAX) * (High-Low) + Low;
}      


REAL RandomNormalREAL(REAL Mu, REAL Sigma)
{
  REAL x,fx;

  do {
    x = RandomEqualREAL(Mu-3*Sigma, Mu+3*Sigma);
    fx = (1 / (sqrt(2*PI)*Sigma)) * exp(-sqr(x-Mu) / (2*sqr(Sigma)));
  } while (fx < RandomEqualREAL(0, 1));
  return x;
}      


/******************************************************************************
               A P P L I C A T I O N - S P E C I F I C   C O D E
 ******************************************************************************/


#define ROWS          25
#define COLS          25

#define N             2
#define C             (ROWS * COLS)
#define M             1

#define TRAIN_STEPS   10000
#define BALANCED      100

FILE*                 f;


void InitializeApplication(NET* Net)
{
  INT i;
   
  for (i=0; i<Net->KohonenLayer->Units; i++) {
    Net->KohonenLayer->StepSize[i] = 1;
    Net->KohonenLayer->dScoreMean[i] = 0;
  }
  f = fopen("SOM.txt", "w");
}


void WriteNet(NET* Net)
{
  INT  r,c;
  REAL x,y,z;

  fprintf(f, "\n\n\n");
  for (r=0; r<ROWS; r++) {
    for (c=0; c<COLS; c++) {
      x = Net->KohonenLayer->Weight[r*ROWS+c][0];
      y = Net->KohonenLayer->Weight[r*ROWS+c][1];
      z = Net->OutputLayer->Weight[0][r*ROWS+c];
      fprintf(f, "([%5.1f°, %5.1f°], %5.1fN) ", x, y, z);
    }
    fprintf(f, "\n");
  }
}


void FinalizeApplication(NET* Net)
{
  fclose(f);
}


/******************************************************************************
                     S I M U L A T I N G   T H E   P O L E
 ******************************************************************************/

                                     /* SIMULATION PARAMETERS FOR THE POLE:   */
#define L             1              /* - length of pole [m]                  */
#define Mc            1              /* - mass of cart [kg]                   */
#define Mp            1              /* - mass of pole [kg]                   */
#define G             9.81           /* - acceleration due to gravity [m/s]  */
#define T             0.1            /* - time step [s]                       */
#define STEPS         10             /* - simulation steps in one time step   */


typedef struct {                     /* STATE VARIABLES OF A POLE:            */
        REAL          x;             /* - position of cart [m]                */
        REAL          xDot;          /* - velocity of cart [m/s]              */
        REAL          w;             /* - angle of pole [degre]                   */
        REAL          wDot;          /* - angular velocity of pole [?s]      */
        REAL          F;             /* - force applied to cart               */
} POLE;                              /*   during current time step [N]        */


void InitializePole(POLE* Pole)
{
  do {
    Pole->x    = 0;
    Pole->xDot = 0;
    Pole->w    = RandomEqualREAL(-30, 30);
    Pole->wDot = 0;
    Pole->F    = 0;
  } while (Pole->w == 0);
}


void SimulatePole(POLE* Pole)
{
  INT  s;
  REAL x, xDot, xDotDot;           
  REAL w, wDot, wDotDot;                   
  REAL F;                    

  x    = Pole->x;
  xDot = Pole->xDot;
  w    = (Pole->w    / 180) * PI;
  wDot = (Pole->wDot / 180) * PI;
  F    = Pole->F;
  for (s=0; s<STEPS; s++) {

    wDotDot = (G*sin(w) + cos(w) * ((-F - Mp*L*sqr(wDot)*sin(w)) / (Mc+Mp))) / 
              (L * ((REAL) 4/3 - (Mp*sqr(cos(w))) / (Mc+Mp)));

    xDotDot = (F + Mp*L * (sqr(wDot)*sin(w) - wDotDot*cos(w))) / (Mc+Mp);

    x    += (T / STEPS) * xDot;
    xDot += (T / STEPS) * xDotDot;
    w    += (T / STEPS) * wDot;
    wDot += (T / STEPS) * wDotDot;
  }
  Pole->x    = x;
  Pole->xDot = xDot;
  Pole->w    = (w    / PI) * 180;
  Pole->wDot = (wDot / PI) * 180;
}


BOOL PoleStillBalanced(POLE* Pole)
{
  return (Pole->w >= -60) AND (Pole->w <= 60);
}


REAL ScoreOfPole(POLE* Pole)
{
  return -sqr(Pole->w);
}


/******************************************************************************
                          I N I T I A L I Z A T I O N
 ******************************************************************************/


void GenerateNetwork(NET* Net)
{
  INT i;

  Net->InputLayer   = (LAYER*) malloc(sizeof(LAYER));
  Net->KohonenLayer = (LAYER*) malloc(sizeof(LAYER));
  Net->OutputLayer  = (LAYER*) malloc(sizeof(LAYER));

  Net->InputLayer->Units        = N;
  Net->InputLayer->Output       = (REAL*)  calloc(N, sizeof(REAL));
      
  Net->KohonenLayer->Units      = C;
  Net->KohonenLayer->Output     = (REAL*)  calloc(C, sizeof(REAL));
  Net->KohonenLayer->Weight     = (REAL**) calloc(C, sizeof(REAL*));
  Net->KohonenLayer->StepSize   = (REAL*)  calloc(C, sizeof(REAL));
  Net->KohonenLayer->dScoreMean = (REAL*)  calloc(C, sizeof(REAL));
      
  Net->OutputLayer->Units       = M;
  Net->OutputLayer->Output      = (REAL*)  calloc(M, sizeof(REAL));
  Net->OutputLayer->Weight      = (REAL**) calloc(M, sizeof(REAL*));
      
  for (i=0; i<C; i++) {
    Net->KohonenLayer->Weight[i] = (REAL*) calloc(N, sizeof(REAL));
  }
  for (i=0; i<M; i++) {
    Net->OutputLayer->Weight[i] = (REAL*) calloc(C, sizeof(REAL));
  }
}


void RandomWeights(NET* Net)
{
  INT i,j;
   
  for (i=0; i<Net->KohonenLayer->Units; i++) {
    for (j=0; j<Net->InputLayer->Units; j++) {
      Net->KohonenLayer->Weight[i][j] = RandomEqualREAL(-30, 30);
    }
  }
  for (i=0; i<Net->OutputLayer->Units; i++) {
    for (j=0; j<Net->KohonenLayer->Units; j++) {
      Net->OutputLayer->Weight[i][j] = 0;
    }
  }
}


void SetInput(NET* Net, REAL* Input)
{
  INT i;
   
  for (i=0; i<Net->InputLayer->Units; i++) {
    Net->InputLayer->Output[i] = Input[i];
  }
}


void GetOutput(NET* Net, REAL* Output)
{
  INT i;
   
  for (i=0; i<Net->OutputLayer->Units; i++) {
    Output[i] = Net->OutputLayer->Output[i];
  }
}


/******************************************************************************
                     P R O P A G A T I N G   S I G N A L S
 ******************************************************************************/


void PropagateToKohonen(NET* Net)
{
  INT  i,j;
  REAL Out, Weight, Sum, MinOut;

  for (i=0; i<Net->KohonenLayer->Units; i++) {
    Sum = 0;
    for (j=0; j<Net->InputLayer->Units; j++) {
      Out = Net->InputLayer->Output[j];
      Weight = Net->KohonenLayer->Weight[i][j];
      Sum += sqr(Out - Weight);
    }
    Net->KohonenLayer->Output[i] = sqrt(Sum);
  }
  MinOut = MAX_REAL;
  for (i=0; i<Net->KohonenLayer->Units; i++) {
    if (Net->KohonenLayer->Output[i] < MinOut)
      MinOut = Net->KohonenLayer->Output[Net->Winner = i];
  }
}


void PropagateToOutput(NET* Net)
{
  INT i;

  for (i=0; i<Net->OutputLayer->Units; i++) {
    Net->OutputLayer->Output[i] = Net->OutputLayer->Weight[i][Net->Winner];
  }
}


void PropagateNet(NET* Net)
{
  PropagateToKohonen(Net);
  PropagateToOutput(Net);
}


/******************************************************************************
                        T R A I N I N G   T H E   N E T
 ******************************************************************************/


REAL Neighborhood(NET* Net, INT i)
{
  INT  iRow, iCol, jRow, jCol;
  REAL Distance;

  iRow = i / COLS; jRow = Net->Winner / COLS;
  iCol = i % COLS; jCol = Net->Winner % COLS;

  Distance = sqrt((double)(sqr(iRow-jRow)) + sqr(iCol-jCol));

  return exp(-sqr(Distance) / (2*sqr(Net->Sigma)));
}


void TrainKohonen(NET* Net, REAL* Input)
{
  INT  i,j;
  REAL Out, Weight, Lambda, StepSize;

  for (i=0; i<Net->KohonenLayer->Units; i++) {
    for (j=0; j<Net->InputLayer->Units; j++) {
      Out = Input[j];
      Weight = Net->KohonenLayer->Weight[i][j];
      Lambda = Neighborhood(Net, i);
      Net->KohonenLayer->Weight[i][j] += Net->Alpha * Lambda * (Out - Weight);
    }
    StepSize = Net->KohonenLayer->StepSize[i];
    Net->KohonenLayer->StepSize[i] += Net->Alpha__ * Lambda * -StepSize;
  }
}


void TrainOutput(NET* Net, REAL* Output)
{
  INT  i,j;
  REAL Out, Weight, Lambda;

  for (i=0; i<Net->OutputLayer->Units; i++) {
    for (j=0; j<Net->KohonenLayer->Units; j++) {
      Out = Output[i];
      Weight = Net->OutputLayer->Weight[i][j];
      Lambda = Neighborhood(Net, j);
      Net->OutputLayer->Weight[i][j] += Net->Alpha_ * Lambda * (Out - Weight);
    }
  }
}


void TrainUnits(NET* Net, REAL* Input, REAL* Output)
{
  TrainKohonen(Net, Input);
  TrainOutput(Net, Output);
}


void TrainNet(NET* Net)
{
  INT  n,t;
  POLE Pole;
  REAL wOld, wNew, ScoreOld, ScoreNew, dScore, dScoreMean, StepSize;
  REAL Input[N];
  REAL Output[M];
  REAL Target[M];

  n = 0;
  while (n<TRAIN_STEPS) {
    t = 0;
    InitializePole(&Pole);
    fprintf(f, " Time     Angle     Force\n");
    fprintf(f, "%4.1fs    %5.1f°  %5.1fN\n", t * T, Pole.w, Pole.F);
    wOld = Pole.w;
    ScoreOld = ScoreOfPole(&Pole);
    SimulatePole(&Pole);
    wNew = Pole.w;
    ScoreNew = ScoreOfPole(&Pole);
    while (PoleStillBalanced(&Pole) AND (t<BALANCED)) {
      n++;
      t++;
      Net->Alpha   = 0.5 * pow(0.01, (REAL) n / TRAIN_STEPS);
      Net->Alpha_  = 0.5 * pow(0.01, (REAL) n / TRAIN_STEPS);
      Net->Alpha__ = 0.005;
      Net->Gamma   = 0.05;
      Net->Sigma   = 6.0 * pow(0.2, (REAL) n / TRAIN_STEPS);
      Input[0] = wOld;
      Input[1] = wNew;
      SetInput(Net, Input);
      PropagateNet(Net);
      GetOutput(Net, Output);
      Pole.F = Output[0];
      StepSize = Net->KohonenLayer->StepSize[Net->Winner];
      Pole.F += StepSize * RandomNormalREAL(0, 10);
      fprintf(f, "%4.1fs    %5.1f°  %5.1fN\n", t * T, Pole.w, Pole.F);
      wOld = Pole.w;
      ScoreOld = ScoreOfPole(&Pole);
      SimulatePole(&Pole);
      wNew = Pole.w;
      ScoreNew = ScoreOfPole(&Pole);
      dScore = ScoreNew - ScoreOld;
      dScoreMean = Net->KohonenLayer->dScoreMean[Net->Winner];
      if (dScore > dScoreMean) {
        Target[0] = Pole.F;
        TrainUnits(Net, Input, Target);
      }
      Net->KohonenLayer->dScoreMean[Net->Winner] += Net->Gamma * (dScore - dScoreMean);
    }
    if (PoleStillBalanced(&Pole))
      fprintf(f, "Pole still balanced after %0.1fs ...\n\n", t * T);
    else
      fprintf(f, "Pole fallen after %0.1fs ...\n\n", (t+1) * T);
  }
}


/******************************************************************************
                                    M A I N
 ******************************************************************************/


void main()
{
  NET Net;

  InitializeRandoms();
  GenerateNetwork(&Net);
  RandomWeights(&Net);
  InitializeApplication(&Net);
  TrainNet(&Net);
  WriteNet(&Net);
  FinalizeApplication(&Net);
}