learn.c

卡耐基.梅隆大学的机器人仿真软件（Redhat linux 9下安装）

#include "learn.h"

#define SQR(A) ((A)*(A))

struct TLog_struct {
  float C, D, T;
  float dist, turn;
};
typedef struct TLog_struct TLog;

FILE *outFile;


inline void LeastSquares(double array[2][2], double out[2])
{
  double x, y;

  x = ((out[0]*array[1][1]) - (out[1]*array[0][1])) / ((array[0][0]*array[1][1]) - (array[0][1]*array[1][0]));
  y = ((array[0][0]*out[1]) - (array[1][0]*out[0])) / ((array[0][0]*array[1][1]) - (array[0][1]*array[1][0]));

  out[0] = x;
  out[1] = y;
}


void Learn(TPath *truePath) 
{
  int cnt;
  double total;
  double md, mt, mC, mD, mT, vd, vt, vC, vD, vT;

  TLog log[MAX_ITERATIONS];
  double meanNormal[2][2], varNormal[2][2];
  double meanC[2], meanD[2], meanT[2], varC[2], varD[2], varT[2];

  TPath *path;

  path = truePath->next;

  // determine mean and var of all input.
  cnt = 0;
  md = mt = mC = mD = mT = 0;
  while (path != NULL) {
    log[cnt].dist = path->dist;
    log[cnt].turn = path->turn;
    log[cnt].C = path->C;
    log[cnt].D = path->D;
    log[cnt].T = path->T;

    md = md + log[cnt].dist;
    mt = mt + log[cnt].turn;
    mC = mC + log[cnt].C;
    mD = mD + log[cnt].D;
    mT = mT + log[cnt].T;

    path = path->next;
    cnt++;
    if (cnt > MAX_ITERATIONS)
      path = NULL;
  }
  total = cnt;

  mt = mt / total;
  md = md / total;
  mC = mC / total;
  mD = mD / total;
  mT = mT / total;

  cnt = 0;
  vd = vt = vC = vD = vT = 0;
  for (cnt=0; cnt < total; cnt++) {
    vd = vd + SQR(md-log[cnt].dist);
    vt = vt + SQR(mt-log[cnt].turn);
    vC = vC + SQR(mC-log[cnt].C);
    vD = vD + SQR(mD-log[cnt].D);
    vT = vT + SQR(mT-log[cnt].T);
  }
  vd = sqrt(vd / total);
  vt = sqrt(vt / total);
  vC = sqrt(vC / total);
  vD = sqrt(vD / total);
  vT = sqrt(vT / total);

  meanNormal[0][0] = 0;
  meanNormal[0][1] = 0;
  meanNormal[1][1] = 0;
  meanC[0] = meanC[1] = 0;
  meanD[0] = meanD[1] = 0;
  meanT[0] = meanT[1] = 0;

  for (cnt=0; cnt < total; cnt++) {
    meanNormal[0][0] = meanNormal[0][0] + SQR(log[cnt].dist);
    meanNormal[0][1] = meanNormal[0][1] + (log[cnt].dist*log[cnt].turn);
    meanNormal[1][1] = meanNormal[1][1] + SQR(log[cnt].turn);

    meanC[0] = meanC[0] + (log[cnt].C * log[cnt].dist);
    meanC[1] = meanC[1] + (log[cnt].C * log[cnt].turn);
    meanD[0] = meanD[0] + (log[cnt].D * log[cnt].dist);
    meanD[1] = meanD[1] + (log[cnt].D * log[cnt].turn);
    meanT[0] = meanT[0] + (log[cnt].T * log[cnt].dist);
    meanT[1] = meanT[1] + (log[cnt].T * log[cnt].turn);
  }

  meanNormal[1][0] = meanNormal[0][1];

  LeastSquares(meanNormal, meanC);
  LeastSquares(meanNormal, meanD);
  LeastSquares(meanNormal, meanT);


  /////////////////////////////////////////////////////////////////////////
  // Second Pass, to compute the variances from our just-computed means  //
  /////////////////////////////////////////////////////////////////////////

  varNormal[0][0] = 0;
  varNormal[0][1] = 0;
  varNormal[1][0] = 0;
  varNormal[1][1] = 0;
  varC[0] = 0;
  varC[1] = 0;
  varD[0] = 0;
  varD[1] = 0;
  varT[0] = 0;
  varT[1] = 0;

  for (cnt=0; cnt < total; cnt++) {
    varNormal[0][0] = varNormal[0][0] + SQR(SQR(log[cnt].dist));
    varNormal[0][1] = varNormal[0][1] + (SQR(log[cnt].dist)*SQR(log[cnt].turn));
    varNormal[1][1] = varNormal[1][1] + SQR(SQR(log[cnt].turn));

    varC[0] = varC[0] + SQR(log[cnt].C - (meanC[0]*log[cnt].dist) - (meanC[1]*log[cnt].turn)) * SQR(log[cnt].dist);
    varC[1] = varC[1] + SQR(log[cnt].C - (meanC[0]*log[cnt].dist) - (meanC[1]*log[cnt].turn)) * SQR(log[cnt].turn);

    varD[0] = varD[0] + SQR(log[cnt].D - (meanD[0]*log[cnt].dist) - (meanD[1]*log[cnt].turn)) * SQR(log[cnt].dist);
    varD[1] = varD[1] + SQR(log[cnt].D - (meanD[0]*log[cnt].dist) - (meanD[1]*log[cnt].turn)) * SQR(log[cnt].turn);

    varT[0] = varT[0] + SQR(log[cnt].T - (meanT[0]*log[cnt].dist) - (meanT[1]*log[cnt].turn)) * SQR(log[cnt].dist);
    varT[1] = varT[1] + SQR(log[cnt].T - (meanT[0]*log[cnt].dist) - (meanT[1]*log[cnt].turn)) * SQR(log[cnt].turn);
  }

  varNormal[1][0] = varNormal[0][1];

  LeastSquares(varNormal, varC);
  LeastSquares(varNormal, varD);
  LeastSquares(varNormal, varT);

  if (varC[0] < 0) varC[0] = 0;
  if (varC[1] < 0) varC[1] = 0;
  if (varD[0] < 0) varD[0] = 0;
  if (varD[1] < 0) varD[1] = 0;
  if (varT[0] < 0) varT[0] = 0;
  if (varT[1] < 0) varT[1] = 0;

  fprintf(outFile, "C: %.4f %.4f  %.4f %.4f   [%.6f %.6f]\n", meanC[0], meanC[1], sqrt(varC[0]), sqrt(varC[1]), varC[0], varC[1]);
  fprintf(outFile, "D: %.4f %.4f  %.4f %.4f   [%.6f %.6f]\n", meanD[0], meanD[1], sqrt(varD[0]), sqrt(varD[1]), varD[0], varD[1]);
  fprintf(outFile, "T: %.4f %.4f  %.4f %.4f   [%.6f %.6f]\n", meanT[0], meanT[1], sqrt(varT[0]), sqrt(varT[1]), varT[0], varT[1]);
  fprintf(outFile, "\n");

  meanC_D = meanC[0];
  meanC_T = meanC[1];
  varC_D = sqrt(varC[0]);
  varC_T = sqrt(varC[1]);
  meanD_D = meanD[0];
  meanD_T = meanD[1];
  varD_D = sqrt(varD[0]);
  varD_T = sqrt(varD[1]);
  meanT_D = meanT[0];
  meanT_T = meanT[1];
  varT_D = sqrt(varT[0]);
  varT_T = sqrt(varT[1]);
}