Go to bottom
/******************************************************************************
===================
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(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 [°] */
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(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);
}
Simulator Output for the Pole Balancing Problem
Time Angle Force
0.0s 9.4° 0.0N
0.1s 9.9° 9.6N
0.2s 10.1° -14.3N
0.3s 11.8° -12.8N
0.4s 19.3° 24.2N
0.5s 27.6° -0.9N
0.6s 34.8° 5.5N
0.7s 44.5° -5.5N
0.8s 57.0° 0.2N
Pole fallen after 0.9s ...
Time Angle Force
0.0s -12.4° 0.0N
0.1s -13.1° -3.5N
0.2s -14.7° -19.2N
0.3s -14.5° 12.1N
0.4s -14.4° -10.5N
0.5s -16.5° -3.4N
0.6s -18.0° -10.7N
0.7s -19.5° 3.3N
0.8s -21.7° -3.3N
0.9s -26.3° -6.4N
1.0s -32.1° 5.0N
1.1s -40.4° 0.3N
1.2s -52.4° -2.8N
Pole fallen after 1.3s ...
Time Angle Force
0.0s 26.2° 0.0N
0.1s 27.4° 2.3N
0.2s 31.0° -8.9N
0.3s 38.2° -5.0N
0.4s 50.0° -6.8N
Pole fallen after 0.5s ...
. . .
. . .
. . .
Time Angle Force
0.0s 23.3° 0.0N
0.1s 24.4° 14.4N
0.2s 26.1° 11.0N
0.3s 27.0° 10.4N
0.4s 27.6° 15.7N
0.5s 27.5° 10.5N
0.6s 26.5° 10.3N
0.7s 25.4° 8.3N
0.8s 24.3° 11.3N
0.9s 22.9° 11.4N
1.0s 20.7° 10.6N
1.1s 17.4° 12.7N
1.2s 12.4° 12.0N
1.3s 4.7° -6.6N
1.4s -3.6° -13.9N
1.5s -8.8° -13.8N
1.6s -10.3° -4.6N
1.7s -9.8° -3.4N
1.8s -9.0° -5.2N
1.9s -7.9° -5.3N
2.0s -5.9° -3.1N
2.1s -3.2° -1.2N
2.2s -0.1° 5.0N
2.3s 2.4° 6.2N
2.4s 3.3° 3.5N
2.5s 2.8° 2.0N
2.6s 1.8° -1.9N
2.7s 0.8° -0.7N
2.8s 0.4° -1.4N
2.9s 0.5° 0.0N
3.0s 0.9° 2.2N
3.1s 1.0° 2.8N
3.2s 0.4° 0.9N
3.3s -0.8° -4.4N
3.4s -1.6° -4.4N
3.5s -1.0° -0.1N
3.6s 0.3° 0.9N
3.7s 1.5° 5.2N
3.8s 1.9° 2.8N
3.9s 1.1° -1.1N
4.0s 0.0° -0.1N
4.1s -0.8° -2.8N
4.2s -1.2° -1.2N
4.3s -1.0° -1.5N
4.4s -0.5° 0.5N
4.5s 0.1° 1.4N
4.6s 0.4° 1.4N
4.7s 0.3° -1.1N
4.8s 0.1° 0.2N
4.9s 0.1° -0.6N
5.0s 0.2° -0.0N
5.1s 0.5° 0.0N
5.2s 0.8° 0.6N
5.3s 1.1° 2.4N
5.4s 1.0° -0.5N
5.5s 0.7° -0.1N
5.6s 0.5° 0.3N
5.7s 0.4° -0.1N
5.8s 0.3° 1.6N
5.9s 0.0° -2.0N
6.0s -0.2° -0.6N
6.1s -0.0° 1.7N
6.2s 0.0° 0.1N
6.3s -0.2° -2.6N
6.4s -0.2° -0.4N
6.5s 0.5° 2.9N
6.6s 0.8° 2.0N
6.7s 0.3° 0.2N
6.8s -0.6° -1.2N
6.9s -1.4° -5.3N
7.0s -1.2° -1.4N
7.1s 0.0° 2.3N
7.2s 1.2° 3.3N
7.3s 1.5° 4.9N
7.4s 0.6° -0.7N
7.5s -1.0° -6.9N
7.6s -1.6° -4.9N
7.7s -0.2° 3.4N
7.8s 1.6° 4.4N
7.9s 2.1° 4.9N
8.0s 1.4° -0.0N
8.1s -0.1° -6.2N
8.2s -0.7° -1.5N
8.3s 0.1° 2.8N
8.4s 0.7° 1.9N
8.5s 0.6° 0.2N
8.6s 0.1° -2.3N
8.7s 0.0° -0.5N
8.8s 0.5° 3.4N
8.9s 0.5° -0.6N
9.0s 0.1° 0.7N
9.1s -0.3° -1.6N
9.2s -0.7° -2.4N
9.3s -0.4° -0.8N
9.4s 0.4° 3.1N
9.5s 0.9° 2.4N
9.6s 0.5° 1.1N
9.7s -0.5° -3.0N
9.8s -1.2° -4.8N
9.9s -0.8° 3.0N
10.0s 0.0° 2.7N
Pole still balanced after 10.0s ...
([ -7.0°, -3.2°], 1.1N) ... ([-35.7°, -39.6°], -11.2N)
([ -6.6°, -2.9°], 1.4N) ... ([-34.9°, -38.7°], -11.2N)
([ -6.1°, -2.5°], 1.7N) ... ([-33.3°, -37.1°], -11.3N)
. .
. .
. .
([ 26.6°, 27.8°], 11.5N) ... ([ 8.6°, 3.4°], 0.6N)
([ 28.6°, 30.0°], 11.6N) ... ([ 9.8°, 4.4°], 1.7N)
([ 29.8°, 31.2°], 11.7N) ... ([ 11.0°, 5.5°], 2.6N)
Go to top 
