SUBDIRS = test
lib_LIBRARIES = mcl
-mcl_SOURCES = mcl.c
+mcl_SOURCES = mcl.c mcl_laser.c
mcl_LIBS = robomath m
-include_HEADERS = mcl.h
+include_HEADERS = mcl.h mcl_laser.h
# -ffast-math is the most important optimization here
CFLAGS := $(filter-out -fno-strict-aliasing,$(CFLAGS)) --std=c99 -D_GNU_SOURCE -ffast-math -O3 -funroll-loops -ftree-vectorize
NumRootInports 0
NumRootOutports 0
ParameterArgumentNames ""
- ComputedModelVersion "1.103"
+ ComputedModelVersion "1.105"
NumModelReferences 0
NumTestPointedSignals 0
}
ModifiedByFormat "%<Auto>"
LastModifiedBy "wsh"
ModifiedDateFormat "%<Auto>"
- LastModifiedDate "Mon Apr 28 16:21:59 2008"
- ModelVersionFormat "1.%<AutoIncrement:103>"
+ LastModifiedDate "Tue Apr 29 11:52:05 2008"
+ ModelVersionFormat "1.%<AutoIncrement:105>"
ConfigurationManager "None"
SimulationMode "normal"
LinearizationMsg "none"
}
System {
Name "mcl_test"
- Location [1684, 55, 2666, 437]
+ Location [358, 561, 1340, 943]
Open on
ModelBrowserVisibility off
ModelBrowserWidth 200
Position [410, 109, 440, 141]
Floating off
Location [475, 726, 1126, 908]
- Open on
+ Open off
NumInputPorts "1"
List {
ListType AxesTitles
Position [370, 49, 400, 81]
Floating off
Location [469, 497, 1123, 665]
- Open on
+ Open off
NumInputPorts "1"
ZoomMode "xonly"
List {
MaskIconOpaque on
MaskIconRotate "none"
MaskIconUnits "normalized"
- MaskValueString "1|1|on"
+ MaskValueString "1|0.01|on"
MaskTabNameString ",,"
System {
Name "Rotating laser"
- Location [84, 67, 1108, 652]
- Open off
+ Location [127, 99, 1151, 684]
+ Open on
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
Position [735, 19, 775, 61]
OutPortSampleTime "1/rps"
}
+ Block {
+ BlockType Selector
+ Name "Selector"
+ Ports [1, 1]
+ Position [585, 91, 625, 129]
+ IndexOptions "Index vector (dialog)"
+ Indices "[1]"
+ OutputSizes "1"
+ InputPortWidth "3"
+ }
Block {
BlockType Sum
Name "Subtract"
DstBlock "Mux"
DstPort 3
}
- Line {
- SrcBlock "Beacon memory1"
- SrcPort 1
- DstBlock "Mux"
- DstPort 2
- }
Line {
SrcBlock "Beacon memory"
SrcPort 1
SrcPort 1
Points [15, 0]
Branch {
- Points [400, 0]
- DstBlock "Individual output"
- DstPort trigger
+ DstBlock "Demux"
+ DstPort 1
}
Branch {
- DstBlock "Demux"
+ DstBlock "Selector"
DstPort 1
}
}
DstBlock "Compare\nTo Constant"
DstPort 1
}
+ Line {
+ SrcBlock "Beacon memory1"
+ SrcPort 1
+ DstBlock "Mux"
+ DstPort 2
+ }
+ Line {
+ SrcBlock "Selector"
+ SrcPort 1
+ Points [140, 0]
+ DstBlock "Individual output"
+ DstPort trigger
+ }
}
}
Block {
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
low "0"
- gain "0"
+ gain "232.844"
high "360"
}
Block {
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
low "0"
- gain "1.5"
+ gain "1.47"
high "3"
}
Block {
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
low "0"
- gain "1.1367"
+ gain "1.5087"
high "2.1"
}
Block {
NumRootInports 0
NumRootOutports 0
ParameterArgumentNames ""
- ComputedModelVersion "1.323"
+ ComputedModelVersion "1.326"
NumModelReferences 0
NumTestPointedSignals 0
}
ModifiedByFormat "%<Auto>"
LastModifiedBy "wsh"
ModifiedDateFormat "%<Auto>"
- LastModifiedDate "Mon Apr 28 17:05:34 2008"
- ModelVersionFormat "1.%<AutoIncrement:323>"
+ LastModifiedDate "Tue Apr 29 11:44:26 2008"
+ ModelVersionFormat "1.%<AutoIncrement:326>"
ConfigurationManager "None"
SimulationMode "normal"
LinearizationMsg "none"
}
System {
Name "reg_mcl"
- Location [1729, 496, 2584, 973]
+ Location [391, 580, 1246, 1057]
Open on
ModelBrowserVisibility off
ModelBrowserWidth 200
MaskIconOpaque on
MaskIconRotate "none"
MaskIconUnits "autoscale"
- MaskValueString "3000|0.01|3/180*pi|2/180*pi"
+ MaskValueString "3000|0.01|5/180*pi|10/180*pi"
MaskTabNameString ",,,"
Port {
PortNumber 1
System {
Name "Robot kinematics"
Location [4, 55, 1176, 412]
- Open off
+ Open on
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
set0ParseKeys "9"
set0Sigs "Switch:o1"
}
- Location [6, 687, 330, 1044]
- Open on
+ Location [6, 641, 330, 998]
+ Open off
NumInputPorts "1"
List {
ListType AxesTitles
--- /dev/null
+% Demonstration of resample algorithm
+
+clc
+m=[ 1 1 5 1 1] % sample weights
+N=length(m);
+Q=[cumsum(m)]
+Q = Q./Q(N)
+xQ = 1:N;
+T=cumsum(-log(rand(1, N+1)))
+T = T./T(length(T))
+xT = 0:N-1;
+
+plot(xQ,Q, 'bo-', xT,T(1:N),'ro-');
+grid
+
+output=zeros(size(m));
+i=0; j=1;
+while i<N,
+ if Q(j)>T(i+1),
+ output(i+1)=j;
+ i=i+1;
+ else
+ j=j+1;
+ end
+end
+output
\ No newline at end of file
#include "simstruc.h"
#include "math.h"
#include "robomath.h"
-#include "mcl.h"
+#include "mcl_laser.h"
#include "stdbool.h"
#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
static void mdlProcessParameters(SimStruct *S)
{
- struct mcl_model *mcl = ssGetPWork(S)[0];
+ struct mcl_laser *l = ssGetPWork(S)[0];
printf("%s\n", __FUNCTION__);
/* Tunable parameters was changed during simulation */
- if (mcl) {
+ if (l) {
printf("changing mcl\n");
- mcl->mov_dnoise = NOISE_XY(S);
- mcl->mov_anoise = NOISE_ANGLE(S);
- mcl->aeval_sigma = AEVAL_SIGMA(S);
+ l->pred_dnoise = NOISE_XY(S);
+ l->pred_anoise = NOISE_ANGLE(S);
+ l->aeval_sigma = AEVAL_SIGMA(S);
}
}
#endif
*/
static void mdlStart(SimStruct *S)
{
- struct mcl_model *mcl;
- struct mcl_angles *angles;
+ struct mcl_laser *l;
+ struct mcl_laser_measurement *last_angles;
void **pwork = ssGetPWork(S);
- mcl = malloc(sizeof(*mcl));
- memset(mcl, 0, sizeof(*mcl));
- pwork[0] = (void*)mcl;
+ l = malloc(sizeof(*l));
+ memset(l, 0, sizeof(*l));
+ pwork[0] = (void*)l;
- angles = malloc(sizeof(*angles));
- memset(angles, 0, sizeof(*angles));
- pwork[1] = (void*)angles;
+ last_angles = malloc(sizeof(*last_angles));
+ memset(last_angles, 0, sizeof(*last_angles));
+ pwork[1] = (void*)last_angles;
/* MCL initialization */
- mcl->beacon_cnt = 3;
- mcl->width = 3.0; /* in m */
- mcl->height = 2.1; /* in m */
+ l->beacon_cnt = 3;
+ l->width = 3.0; /* in m */
+ l->height = 2.1; /* in m */
/* the noises */
- mcl->mov_dnoise = NOISE_XY(S);
- mcl->mov_anoise = NOISE_ANGLE(S);
- mcl->w_min = 0.25;
- mcl->w_max = 2.0;
- mcl->eval_sigma = 160;
- mcl->aeval_sigma = AEVAL_SIGMA(S);
- mcl->maxavdist = 0.150;
- /* bad cycles before reseting */
- mcl->maxnoisecycle = 10;
-
- /* amount of particles */
- mcl->count = PART_COUNT(S);
-
- mcl->parts = (struct mcl_particle *)
- malloc(sizeof(struct mcl_particle)*mcl->count);
-
- /* phases of the MCL */
- mcl->init = mcl_init;
- mcl->move = mcl_move;
- mcl->update = mcl_update2; /* angles evalution oriented update */
- mcl->normalize = mcl_normalize;
- mcl->sort = mcl_partsort;
- mcl->resample = mcl_resample;
-
- /* cycles of enumeration */
- mcl->cycle = 0;
- mcl->noisecycle = 0;
-
- /* change flag */
- mcl->flag = MCL_RUN;
-
- /* generate particles with noises */
- mcl->init(mcl);
+ l->pred_dnoise = NOISE_XY(S);
+ l->pred_anoise = NOISE_ANGLE(S);
+ l->aeval_sigma = AEVAL_SIGMA(S);
+
+ mcl_laser_init(l, PART_COUNT(S));
}
#endif /* MDL_START */
ssPrintf("Update %d %g\n", tid, ssGetT(S));
}
-static do_move(struct mcl_model *mcl, InputRealPtrsType iPtrsMove)
-{
- double dx, dy, dangle;
- dx = *iPtrsMove[0];
- dy = *iPtrsMove[1];
- dangle = *iPtrsMove[2];
- mcl->move(mcl, dx, dy, dangle);
-}
-
-static do_measure(struct mcl_model *mcl, struct mcl_angles angles)
-{
- mcl->update(mcl, &angles);
- mcl->normalize(mcl);
- mcl->sort(mcl);
- mcl->resample(mcl);
-}
-
/* Function: mdlOutputs =======================================================
* Abstract:
*/
static void mdlOutputs(SimStruct *S, int_T tid)
{
- struct mcl_model *mcl = ssGetPWork(S)[0];
- struct mcl_angles *last_angles = ssGetPWork(S)[1];
- struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
+ struct mcl_laser *l = ssGetPWork(S)[0];
+ struct mcl_model *mcl = &l->mcl;
+ struct mcl_laser_measurement *last_angles = ssGetPWork(S)[1];
+ struct mcl_laser_state *parts = l->parts;
double *pos = ssGetOutputPortRealSignal(S,OUTPUT_EST_POS);
double *bitmap = ssGetOutputPortRealSignal(S,OUTPUT_BITMAP);
/* ssPrintf("=============================================\n"); */
if (ssIsSampleHit(S, moveTid, tid)) {
/* ssPrintf("move \n"); */
- do_move(mcl, iPtrsMove);
- }
+ struct mcl_laser_state d;
+ d.x = *iPtrsMove[0];
+ d.y = *iPtrsMove[1];
+ d.angle = *iPtrsMove[2];
+ mcl->predict(mcl, &d);
+ }
if (ssIsSampleHit(S, measureTid, tid)) {
- struct mcl_angles angles;
+ struct mcl_laser_measurement angles;
int i;
bool nonzero = false;
bool changed = false;
/* for (i=0; i<angles.count; i++) printf(" a=%5.0f deg", angles.val[i]/M_PI*180); */
/* printf("\n"); */
- do_measure(mcl, angles);
+ mcl->update(mcl, &angles);
+ mcl->resample(mcl);
}
*last_angles = angles;
}
/* Update probability bitmap */
for (i=0; i<BITMAP_WIDTH*BITMAP_HEIGHT; i++)
bitmap[i] = 0;
+ double max = 0;
+ for (i=0; i<mcl->count; i++)
+ max = fmax(max, mcl->weight[i]);
for (i=0; i<mcl->count; i++) {
int x, y;
double color;
- x = parts[i].x / mcl->width * BITMAP_WIDTH;
- y = parts[i].y / mcl->height * BITMAP_HEIGHT;
+ x = parts[i].x / l->width * BITMAP_WIDTH;
+ y = parts[i].y / l->height * BITMAP_HEIGHT;
y = BITMAP_HEIGHT - 1 - y;
if (x>=0 && x<BITMAP_WIDTH && y>=0 && y<BITMAP_HEIGHT) {
- color = 64.0*parts[i].weight;
+ color = 64.0*log(1+mcl->weight[i])/log(1+max);
color = fmax(color, 16);
bitmap[x*BITMAP_HEIGHT+y] = color;
}
/* get the estimated position */
/* ssPrintf("pos: %g %g %g", mcl->est_pos.x, mcl->est_pos.y, mcl->est_pos.angle); */
- pos[0] = mcl->est_pos.x;
- pos[1] = mcl->est_pos.y;
- pos[2] = mcl->est_pos.angle;
+ struct mcl_laser_state *est = mcl->estimated;
+ pos[0] = est->x;
+ pos[1] = est->y;
+ pos[2] = est->angle;
}
/* Function: mdlTerminate =====================================================
#endif
/**
- * IMPLEMENTATION OF THE MONTE CARLO ALGORITHM.
+ * IMPLEMENTATION OF THE GENERIC PART OF MONTE CARLO ALGORITHM.
*/
-/**
- * The prediction phase:
- * Move particles and add some noise to simulate the real movement.
- *
- * @param dx movement in axis X
- * @param dy movement in axis Y
- * @param dangle turning
- */
-void mcl_move(struct mcl_model *mcl, double dx, double dy, double dangle)
-{
- struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
- /*struct mcl_particle robot = *(struct mcl_particle *)mcl->system;*/
- int i=0;
- double xdiff, ydiff, a;
-
- /* move the particles with noises */
- for (i=0; i<mcl->count; i++) {
- double a = parts[i].angle;
- xdiff = dx*cos(a) - dy*sin(a);
- ydiff = dy*cos(a) + dx*sin(a);
-
- parts[i].x += xdiff + mcl->mov_dnoise*gaussian_random();
- parts[i].y += ydiff + mcl->mov_dnoise*gaussian_random();
- parts[i].angle += dangle + mcl->mov_anoise*gaussian_random();
-
- /*printf("x=%f y=%f w=%f r.x=%f r.y=%f\n",
- parts[i].x, parts[i].y, parts[i].weight,
- robot.x, robot.y);*/
- }
-
- /* Update the last estimated position without noise */
- a = mcl->est_pos.angle;
- mcl->est_pos.x += dx*cos(a) - dy*sin(a);
- mcl->est_pos.y += dx*sin(a) + dy*cos(a);
- mcl->est_pos.angle += dangle;
-}
-
-/**
- * First method:
- * Evaluation of the position.
- * The update phase:
- * check the distance of predicted and measured states and evaluate
- * particle`s weight.
- *
- * @param mcl the MCL model
- * @param data measured data (x, y, turning)
+/**
+ * Initializes ::mcl_model and allocates neccessary memory. Should be
+ * called from implementation init function.
+ *
+ * @param mcl MCL model structure
+ * @param part_size Size of one particle
+ * @param count The number of particles
*/
-void mcl_update(struct mcl_model *mcl, void *data)
+void mcl_init(struct mcl_model *mcl, unsigned count, void *estimated)
{
- struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
- double xdiff, ydiff, dist, avdist=0;
- int i;
- double x = ((double *)data)[0];
- double y = ((double *)data)[1];
- /*double angle = ((double *)data)[2];*/
-
- /* evaluate the weights of each particle */
- for (i=0; i<mcl->count; i++) {
- xdiff = parts[i].x - x;
- ydiff = parts[i].y - y;
- dist = fabs(sqrt(xdiff*xdiff + ydiff*ydiff));
- avdist += dist;
-
- /* evaluate weights with gaussian probability density */
- parts[i].weight *= evaluate_gaussian(dist, mcl->eval_sigma);
- }
-
- /* adjust the evaluation values */
- avdist /= mcl->count;
- if (avdist > mcl->maxavdist) {
- if (mcl->noisecycle > mcl->maxnoisecycle) {
- mcl->flag = MCL_RESET;
- return;
- } else
- mcl->noisecycle++;
- }
+ mcl->count = count;
+ mcl->weight = malloc(count*sizeof(*mcl->weight));
+ mcl->estimated = estimated;
}
-/**
- * Second method:
- * Evalution of the angles between the robot's head and reflectors.
- * The update phase:
- * Evaluate particle's weight using measured angles.
- *
- * @param mcl the MCL model
- * @param data measured angles (angles[0] is the angles count)
+/**
+ * Deallocates memory allocated by mcl_init().
+ *
+ * @param mcl MCL model structure.
*/
-#ifdef MATLAB_MEX_FILE
-#define S_FUNCTION_LEVEL 2
-#define S_FUNCTION_NAME sf_mcl
-#include "simstruc.h"
-#endif
-
-static inline double gaussian(double val, double sigma)
+void mcl_done(struct mcl_model *mcl)
{
- return exp(-0.5 * (val*val / (sigma*sigma)));
-}
-
-
-void mcl_update2(struct mcl_model *mcl, void *data)
-{
- struct mcl_particle * restrict parts = (struct mcl_particle *)mcl->parts;
- const struct mcl_angles *angles = (struct mcl_angles*)data;
- mcl_thetas theta;
- double p;
- double diff;
- int i, it, im;
-
- /* evaluate the weights of each particle */
- for (i=0; i<mcl->count; i++) {
- mcl_pos2ang(&parts[i], theta, mcl->beacon_cnt, mcl->beacon_color);
- parts[i].weight = 1; /* It seems resample doesn't set weight to 1 */
-
- if (parts[i].x < 0 || parts[i].x > mcl->width ||
- parts[i].y < 0 || parts[i].y > mcl->height) {
- /* We cannot be out of playground */
- parts[i].weight = 0;
- continue;
- }
-
- for (im=0; im<angles->count; im++) {
- p=0;
- for (it=0; it<mcl->beacon_cnt; it++) {
- diff = fabs(theta[it] - angles->val[im]);
- if (diff > M_PI) diff = 2*M_PI-diff;
- p += evaluate_gaussian(diff, 2*mcl->aeval_sigma);
- }
- parts[i].weight *= p;
- }
- }
-
- /* adjust the evaluation values */
-#if 0
-// avdist /= mcl->count;
-// if (avdist > mcl->maxavdist) {
-// if (mcl->noisecycle > mcl->maxnoisecycle) {
-// mcl->flag = MCL_RESET;
-// return;
-// } else
-// mcl->noisecycle++;
-// }
-#endif
+ free(mcl->weight);
}
/**
- * Normalize the particles.
+ * Normalize the particle weights.
*
* @param mcl the MCL model
*/
void mcl_normalize(struct mcl_model *mcl)
{
- struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
- double gain=0, w=0;
+ double sum=0;
int i;
for (i=0; i<mcl->count; i++)
- w += parts[i].weight;
- gain = mcl->count / w;
-
- if (gain <= 0)
- return;
+ sum += mcl->weight[i];
for (i=0; i<mcl->count; i++)
- parts[i].weight *= gain;
+ mcl->weight[i] /= sum;
}
-/* Compare function. Used to compare the weight of particles. Sorting is
- * realised with the qsort() function. */
-int compare(const void *a, const void *b)
+#if 0
+void mcl_resample_old(struct mcl_model *mcl)
{
- return ((double)(((struct mcl_particle *)a)->weight) >
- (double)(((struct mcl_particle *)b)->weight));
-}
-
-/**
- * Sort the particles by the weight.
- *
- * @param *data data to be compared
- */
-void mcl_partsort(struct mcl_model *mcl)
-{
- struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
- qsort(parts, mcl->count, sizeof(struct mcl_particle), compare);
-}
-
-/**
- * Resample the particles and eliminate the samples with too low or too
- * high weights.
- *
- * @param mcl the MCL model
- */
-void mcl_resample(struct mcl_model *mcl)
-{
- struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
+ void *parts = (struct mcl_particle *)mcl->parts;
int i, j;
int i_max=0, i_min=0;
size_t num = 0;
mcl->est_pos.y = best.y;
mcl->est_pos.angle = best.angle;
}
-
-/**
- * Generate particles with noises. Initialization of the MCL.
- *
- * @param mcl the MCL model
- */
-void mcl_init (struct mcl_model *mcl)
-{
- struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
- int i;
-
- /*int xgridsize, ygridsize;*/
-
- /*
- ygridsize = (int)sqrt((double)((double)mcl->count*(double)mcl->height/
- (double)mcl->width));
- xgridsize = (int)((double)mcl->width/(double)mcl->height*ygridsize);
- printf("xgridsize=%d ygridsize=%d\n", xgridsize, ygridsize);*/
-
- if (mcl->beacon_cnt > BEACON_CNT)
- mcl->beacon_cnt = BEACON_CNT;
-
- /* particles initialization */
- for (i=0; i<mcl->count; i++) {
- /*xdiff = 5*cos(parts[i].angle) + 5*sin(parts[i].angle);
- ydiff = 5*cos(parts[i].angle) + 5*sin(parts[i].angle);
-
- parts[i].x = mcl->system->x + xdiff +
- POS_STDEV_FRAC*xdiff*gaussian_random();
- parts[i].y = mcl->system->y + ydiff +
- POS_STDEV_FRAC*ydiff*gaussian_random();
- parts[i].angle += dangle + POS_STDEV_FRAC*dangle*gaussian_random();
- */
-
- /* generate particles with noises. Starting weight is 1, as we
- * consider that any of the particles could be the right one.
- */
- parts[i].x = mcl->xoff + (rand()&0xffff)/65535.0*mcl->width;
- parts[i].y = mcl->yoff + (rand()&0xffff)/65535.0*mcl->height;
- parts[i].angle = (rand() % 360)/360.0*(2.0*M_PI);
- parts[i].weight = 1;
-
- /*
- parts[i].x = (i%xgridsize)*(mcl->width/xgridsize)+xgridsize/2;
- parts[i].y = (i/xgridsize)*(mcl->height/ygridsize);
- parts[i].angle = rand() % (int)mcl->gen_anoise;
- parts[i].weight = 1;*/
-
- /*printf("x=%f, y=%f, a=%f, w=%f\n",
- parts[i].x, parts[i].y,
- parts[i].angle, parts[i].weight);*/
- }
-}
-
-/**
- * Get position of the particle with highest weight.
- *
- * @param mcl the MCL model
- */
-inline struct mcl_particle mcl_get_pos(struct mcl_model *mcl)
-{
- return (((struct mcl_particle *)mcl->parts)[mcl->count-1]);
-}
-
-
-
-/**
- * Convert the position to angles between robot's head and reflectors
- * (angles sense is counter-clockwise)
- *
- * @param part MCL particle
- * @param theta Returned angles to reflectors
- * @param color beacon color
- * @param cnt beacon count
- */
-void mcl_pos2ang(const struct mcl_particle *part,
- mcl_thetas theta,
- unsigned char cnt, unsigned char color)
-{
- double head;
- int i;
-
- if (cnt > BEACON_CNT)
- cnt = BEACON_CNT;
-
- if (color == BEACON_RED)
- head = part->angle + M_PI;
- else
- head = part->angle;
-
- head = (head < 0) ? 2*M_PI + head : head;
-
- /* angles between 0 and reflectors (with center at robot position) */
- if (color == BEACON_RED) {
- theta[0] = atan2(BEACON_RED1_Y-part->y, BEACON_RED1_X-part->x);
- theta[1] = atan2(BEACON_RED2_Y-part->y, BEACON_RED2_X-part->x);
- theta[2] = atan2(BEACON_RED3_Y-part->y, BEACON_RED3_X-part->x);
- theta[3] = atan2(BEACON_RED4_Y-part->y, BEACON_RED4_X-part->x);
- } else {
- theta[0] = atan2(BEACON_BLUE1_Y-part->y, BEACON_BLUE1_X-part->x);
- theta[1] = atan2(BEACON_BLUE2_Y-part->y, BEACON_BLUE2_X-part->x);
- theta[2] = atan2(BEACON_BLUE3_Y-part->y, BEACON_BLUE3_X-part->x);
- theta[3] = atan2(BEACON_BLUE4_Y-part->y, BEACON_BLUE4_X-part->x);
- }
-
- for (i=0; i<cnt; i++) {
- /* normalize angles */
- if (theta[i] < 0) theta[i] = 2*M_PI + theta[i];
- theta[i] = (theta[i]-head < 0) ?
- 2*M_PI+theta[i]-head : theta[i]-head;
- }
-
- /* sort and store results */
- qsort(theta, cnt, sizeof(double), cmpd);
-}
+#endif
/**
* End of the MCL stuffs.
#ifndef __mcl__
#define __mcl__
-#define POS_STDEV_FRAC 2
-
-#include <robodim_eb2008.h>
-
-/* MCL sample */
-struct mcl_particle {
- double x;
- double y;
- double angle;
- double weight;
-};
-
-/* Estimated position */
-struct mcl_position {
- double x;
- double y;
- double angle;
-};
-
-typedef double mcl_thetas[BEACON_CNT];
-
-/* Measured angles */
-struct mcl_angles {
- int count;
- double val[10];
-};
+typedef float mcl_weight_t;
struct mcl_model {
/* operations with particles */
- void (*init) (struct mcl_model *mcl);
- void (*move) (struct mcl_model *mcl, double dx, double dy, double dangle);
- void (*update) (struct mcl_model *mcl, void *data);
- void (*normalize) (struct mcl_model *mcl);
- void (*sort) (struct mcl_model *mcl);
+
+ /** Resets particles to the initial (random) state */
+ void (*reset) (struct mcl_model *mcl);
+ /** Prediction step of MCL. Moves particles by delta +
+ * noise. Also updates the estimated field by delta (without
+ * noise). */
+ void (*predict) (struct mcl_model *mcl, void *delta);
+ /** Update step. Sets particle weights acording to the
+ * measurement. Updates estimated field to match the particle
+ * with the highest weight. */
+ void (*update) (struct mcl_model *mcl, void *measurement);
+ /** Normalization and resampling. */
void (*resample) (struct mcl_model *mcl);
- struct mcl_position est_pos;
+ /** Deallocates all memory etc. */
+ void (*done) (struct mcl_model *mcl);
+
+ void *estimated; /* current best estimation of state */
+
/* particles */
- void *parts; /* pointer to particles */
int count; /* number of particles */
- /* the system (robot, ...) */
- void *system; /* system model */
- /* noises: movement noise, */
- double gen_dnoise; /* dispersion of distance noise to generate */
- double gen_anoise; /* dispersion of angles noise to generate */
- double mov_dnoise; /* movement distance noises */
- double mov_anoise; /* movement angle noises */
- /* used to evaluate the particles */
- double w_min; /* minimal probability used for resampling */
- double w_max; /* maximum probability used for resampling */
- double eval_sigma; /* sigma used for evaluating */
- double aeval_sigma; /* sigma used for angles evaluating */
- /* size of the playground (in meters) */
- double width;
- double height;
- double xoff; /* X offset */
- double yoff; /* Y offset */
- /* counters stuff */
- int cycle; /* counter of updated cycles */
- int noisecycle; /* counter of noise cycles */
- /* used to reinitialisation after too many bad measures */
- int maxnoisecycle; /* max. number of noise cycles before reinit. */
- double maxavdist; /* distance to be considered as noise */
- unsigned char flag; /* program flag, see below */
- /* measured data */
- void *data;
- unsigned char beacon_cnt; /* number of beacons */
- unsigned char beacon_color; /* red/blue beacons */
-};
-
-enum {
- MCL_STOP = 0,
- MCL_RUN = 1,
- MCL_RESET = 255
+ mcl_weight_t *weight; /* pointer to particle weights */
+ void *priv; /* private data for implementation */
};
/**
*/
#ifdef __cplusplus
extern "C" {
-#endif
-void mcl_move(struct mcl_model *mcl, double dx, double dy, double dangle);
-void mcl_update(struct mcl_model *mcl, void *data);
-void mcl_update2(struct mcl_model *mcl, void *data);
+#endif
+void mcl_init(struct mcl_model *mcl, unsigned count, void *estimated);
+void mcl_done(struct mcl_model *mcl);
void mcl_normalize(struct mcl_model *mcl);
-void mcl_partsort(struct mcl_model *mcl);
-void mcl_resample(struct mcl_model *mcl);
-void mcl_init (struct mcl_model *mcl);
-inline struct mcl_particle mcl_get_pos(struct mcl_model *mcl);
-void mcl_pos2ang(const struct mcl_particle *part,
- mcl_thetas theta,
- unsigned char cnt, unsigned char color);
#ifdef __cplusplus
}
#endif
--- /dev/null
+#include "mcl_laser.h"
+#include <stdlib.h>
+#include <math.h>
+#include <robomath.h>
+
+#define mcl_to_laser(mcl) (struct mcl_laser*)((mcl)->priv)
+
+#ifdef MATLAB_MEX_FILE
+#define S_FUNCTION_LEVEL 2
+#define S_FUNCTION_NAME sf_mcl
+#include "simstruc.h"
+#endif
+
+
+static void mcl_laser_reset(struct mcl_model *mcl)
+{
+ struct mcl_laser *l = mcl_to_laser(mcl);
+ struct mcl_laser_state *parts = l->parts;
+ int i;
+
+ if (l->beacon_cnt > BEACON_CNT)
+ l->beacon_cnt = BEACON_CNT;
+
+ /* particles initialization */
+ for (i=0; i<mcl->count; i++) {
+ /* generate particles with noises. Starting weight is 1, as we
+ * consider that any of the particles could be the right one.
+ */
+ parts[i].x = (rand()&0xffff)/65535.0*l->width;
+ parts[i].y = (rand()&0xffff)/65535.0*l->height;
+ parts[i].angle = (rand() % 360)/360.0*(2.0*M_PI);
+ }
+
+}
+
+static void mcl_laser_predict(struct mcl_model *mcl, void *delta)
+{
+ struct mcl_laser *l = mcl_to_laser(mcl);
+ struct mcl_laser_state *parts = l->parts;
+ struct mcl_laser_state *d = delta;
+ struct mcl_laser_state *estimated = mcl->estimated;
+
+ int i=0;
+ double xdiff, ydiff, a;
+
+ /* move the particles with noises */
+ for (i=0; i<mcl->count; i++) {
+ double a = parts[i].angle;
+ xdiff = d->x*cos(a) - d->y*sin(a);
+ ydiff = d->y*cos(a) + d->x*sin(a);
+
+ parts[i].x += xdiff + l->pred_dnoise*gaussian_random();
+ parts[i].y += ydiff + l->pred_dnoise*gaussian_random();
+ parts[i].angle += d->angle + l->pred_anoise*gaussian_random();
+
+ /*printf("x=%f y=%f w=%f r.x=%f r.y=%f\n",
+ parts[i].x, parts[i].y, parts[i].weight,
+ robot.x, robot.y);*/
+ }
+
+ /* Update the last estimated position without noise */
+ a = estimated->angle;
+ estimated->x += d->x*cos(a) - d->y*sin(a);
+ estimated->y += d->x*sin(a) + d->y*cos(a);
+ estimated->angle += d->angle;
+}
+
+static inline double gaussian(double val, double sigma)
+{
+ return exp(-0.5 * (val*val / (sigma*sigma)));
+}
+
+
+static void mcl_laser_update(struct mcl_model *mcl, void *measurement)
+{
+ struct mcl_laser *l = mcl_to_laser(mcl);
+ const struct mcl_laser_state * restrict parts = l->parts;
+ const struct mcl_laser_state * restrict best = &parts[0];
+ const struct mcl_laser_measurement *angles = measurement;
+ mcl_thetas theta;
+ double p;
+ double diff;
+ int i, it, im;
+ mcl_weight_t wmax = 0;
+
+ /* evaluate the weights of each particle */
+ for (i=0; i<mcl->count; i++) {
+ mcl_pos2ang(&parts[i], theta, l->beacon_cnt, l->beacon_color);
+ if (parts[i].x < 0 || parts[i].x > l->width ||
+ parts[i].y < 0 || parts[i].y > l->height) {
+ /* We cannot be out of playground */
+ mcl->weight[i] = 0;
+ continue;
+ }
+
+ mcl->weight[i] = 1;
+ for (im=0; im<angles->count; im++) {
+ p=0;
+ for (it=0; it<l->beacon_cnt; it++) {
+ diff = fabs(theta[it] - angles->val[im]);
+ if (diff > M_PI) diff = 2*M_PI-diff;
+ p += gaussian(diff, 2*l->aeval_sigma);
+ }
+ mcl->weight[i] *= p;
+ }
+ if (mcl->weight[i] > wmax) {
+ wmax = mcl->weight[i];
+ best = &parts[i];
+ }
+ }
+ *(struct mcl_laser_state*)mcl->estimated = *best;
+}
+
+
+static inline double rnd()
+{
+ return 1 - drand48(); /* exclude zero */
+}
+
+/**
+ * Resample the particles. Mystic alg. 1 from "Improved particle
+ * filter for nonlinear problems".
+ *
+ * @note It already contains normalization so it is not neccessary to
+ * call mcl_normalize().
+ *
+ * @param mcl the MCL model
+ */
+static void mcl_laser_resample(struct mcl_model *mcl)
+{
+ struct mcl_laser *l = mcl_to_laser(mcl);
+ struct mcl_laser_state *parts = l->parts;
+ struct mcl_laser_state *resampled = l->resampled;
+ mcl_weight_t *Q = l->Q, *T=l->T, *weight = mcl->weight;
+
+ float sum;
+ int i, j;
+
+ /* Q is shifted one element to the left sice the initial zero
+ * is never accessed. */
+ Q[0] = weight[0];
+ for (i = 1; i <= mcl->count; i++)
+ Q[i] = Q[i-1] + weight[i];
+
+ /* We normalize here since Q should be now in cache and we
+ * also save one pass as compared to standalone
+ * mcl_normalize(). */
+ sum = Q[mcl->count];
+ for (i = 1; i <= mcl->count; i++)
+ Q[i] /= sum;
+
+ /* We do not store the last element of T since it is always
+ * one and during resample loop this element is never
+ * reached. */
+ T[0] = -log(rnd());
+ for (i = 1; i < mcl->count; i++)
+ T[i] = T[i-1] - log(rnd());
+ sum = T[mcl->count-1] - log(rnd());
+ for (i = 0; i < mcl->count; i++)
+ T[i] /= sum;
+
+ for (i = 0, j = 0; i < mcl->count; ) {
+ if (Q[j] > T[i]) {
+ resampled[i] = parts[j];
+ ++i;
+ }
+ else
+ ++j;
+ }
+
+ /* Swap resampled and non-resampled particles */
+ l->parts = resampled;
+ l->resampled = parts;
+}
+
+void mcl_laser_done(struct mcl_model *mcl)
+{
+ struct mcl_laser *l = mcl_to_laser(mcl);
+
+ mcl_done(mcl);
+ free(l->parts);
+ free(l->resampled);
+ free(l->Q);
+ free(l->T);
+}
+
+
+struct mcl_model *mcl_laser_init(struct mcl_laser *l, unsigned count)
+{
+ struct mcl_model *mcl = &l->mcl;
+
+ mcl_init(mcl, count, &l->estimated);
+ mcl->priv = l;
+
+ l->parts = malloc(count*sizeof(struct mcl_laser_state));
+ l->resampled = malloc(count*sizeof(struct mcl_laser_state));
+ l->Q = malloc(count*sizeof(*l->Q));
+ l->T = malloc(count*sizeof(*l->T));
+
+ if (l->beacon_cnt > BEACON_CNT)
+ l->beacon_cnt = BEACON_CNT;
+
+ /* Initialize method pointers */
+ mcl->reset = mcl_laser_reset;
+ mcl->predict = mcl_laser_predict;
+ mcl->update = mcl_laser_update;
+ mcl->resample = mcl_laser_resample;
+ mcl->done = mcl_laser_done;
+
+ mcl_laser_reset(mcl);
+ return mcl;
+}
+
+/**
+ * Convert the position to angles between robot's head and reflectors
+ * (angles sense is counter-clockwise)
+ *
+ * @param part MCL particle
+ * @param theta Returned angles to reflectors
+ * @param color beacon color
+ * @param cnt beacon count
+ */
+void mcl_pos2ang(const struct mcl_laser_state *part,
+ mcl_thetas theta,
+ unsigned char cnt, unsigned char color)
+{
+ double head;
+ int i;
+
+ if (cnt > BEACON_CNT)
+ cnt = BEACON_CNT;
+
+ if (color == BEACON_RED)
+ head = part->angle + M_PI;
+ else
+ head = part->angle;
+
+ head = (head < 0) ? 2*M_PI + head : head;
+
+ /* angles between 0 and reflectors (with center at robot position) */
+ if (color == BEACON_RED) {
+ theta[0] = atan2(BEACON_RED1_Y-part->y, BEACON_RED1_X-part->x);
+ theta[1] = atan2(BEACON_RED2_Y-part->y, BEACON_RED2_X-part->x);
+ theta[2] = atan2(BEACON_RED3_Y-part->y, BEACON_RED3_X-part->x);
+ theta[3] = atan2(BEACON_RED4_Y-part->y, BEACON_RED4_X-part->x);
+ } else {
+ theta[0] = atan2(BEACON_BLUE1_Y-part->y, BEACON_BLUE1_X-part->x);
+ theta[1] = atan2(BEACON_BLUE2_Y-part->y, BEACON_BLUE2_X-part->x);
+ theta[2] = atan2(BEACON_BLUE3_Y-part->y, BEACON_BLUE3_X-part->x);
+ theta[3] = atan2(BEACON_BLUE4_Y-part->y, BEACON_BLUE4_X-part->x);
+ }
+
+ for (i=0; i<cnt; i++) {
+ /* normalize angles */
+ if (theta[i] < 0) theta[i] = 2*M_PI + theta[i];
+ theta[i] = (theta[i]-head < 0) ?
+ 2*M_PI+theta[i]-head : theta[i]-head;
+ }
+
+ /* sort and store results */
+ qsort(theta, cnt, sizeof(double), cmpd);
+}
--- /dev/null
+#ifndef MCL_LASER_H
+#define MCL_LASER_H
+
+#include <mcl.h>
+#include <robodim_eb2008.h>
+
+#define MCL_LASER_MAX_MEASURE 10
+
+struct mcl_laser_state {
+ double x;
+ double y;
+ double angle;
+};
+
+struct mcl_laser_measurement {
+ int count;
+ double val[MCL_LASER_MAX_MEASURE];
+};
+
+typedef double mcl_thetas[BEACON_CNT];
+
+struct mcl_laser {
+ struct mcl_model mcl;
+ struct mcl_laser_state *parts; /* pointer to particles */
+ struct mcl_laser_state estimated; /* current best estimated position */
+ /* the system (robot, ...) */
+ void *system; /* system model */
+ /* noises: movement noise, */
+ double pred_dnoise; /* prediction distance noises */
+ double pred_anoise; /* prediction angle noises */
+ /* used to evaluate the particles */
+ double aeval_sigma; /* sigma used for angles evaluating */
+ /* size of the playground (in meters) */
+ double width;
+ double height;
+#if 0
+ /* counters stuff */
+ int cycle; /* counter of updated cycles */
+ int noisecycle; /* counter of noise cycles */
+ /* used to reinitialisation after too many bad measures */
+ int maxnoisecycle; /* max. number of noise cycles before reinit. */
+ double maxavdist; /* distance to be considered as noise */
+ unsigned char flag; /* program flag, see below */
+ /* measured data */
+ void *data;
+#endif
+ unsigned char beacon_cnt; /* number of beacons */
+ unsigned char beacon_color; /* red/blue beacons */
+
+ /* Temporary arrays for resampling */
+ mcl_weight_t *Q, *T;
+ struct mcl_laser_state *resampled;
+};
+
+
+struct mcl_model *mcl_laser_init(struct mcl_laser *l, unsigned count);
+
+void mcl_pos2ang(const struct mcl_laser_state *part,
+ mcl_thetas theta,
+ unsigned char cnt, unsigned char color);
+
+#endif
--- /dev/null
+/**
+ * First method:
+ * Evaluation of the position.
+ * The update phase:
+ * check the distance of predicted and measured states and evaluate
+ * particle`s weight.
+ *
+ * @param mcl the MCL model
+ * @param data measured data (x, y, turning)
+ */
+void mcl_laser_pos(struct mcl_model *mcl, void *data)
+{
+ struct mcl_particle *parts = (struct mcl_particle *)mcl->parts;
+ double xdiff, ydiff, dist, avdist=0;
+ int i;
+ double x = ((double *)data)[0];
+ double y = ((double *)data)[1];
+ /*double angle = ((double *)data)[2];*/
+
+ /* evaluate the weights of each particle */
+ for (i=0; i<mcl->count; i++) {
+ xdiff = parts[i].x - x;
+ ydiff = parts[i].y - y;
+ dist = fabs(sqrt(xdiff*xdiff + ydiff*ydiff));
+ avdist += dist;
+
+ /* evaluate weights with gaussian probability density */
+ parts[i].weight *= evaluate_gaussian(dist, mcl->eval_sigma);
+ }
+
+ /* adjust the evaluation values */
+ avdist /= mcl->count;
+ if (avdist > mcl->maxavdist) {
+ if (mcl->noisecycle > mcl->maxnoisecycle) {
+ mcl->flag = MCL_RESET;
+ return;
+ } else
+ mcl->noisecycle++;
+ }
+}
+
#include <stdlib.h>
#include <time.h>
#include <sys/time.h>
-#include <mcl.h>
+#include <mcl_laser.h>
#include <math.h>
#include <robomath.h>
#include <robodim_eb2007.h>
#define HIST_MIN 0.000001
#define HIST_MAX 1.0
-struct mcl_model mcl;
-struct mcl_angles meas_angles;
-struct mcl_particle act_pos;
-struct mcl_particle est_pos;
-struct mcl_particle meas_pos;
-mcl_thetas theta;
-struct mcl_particle *parts;
+struct mcl_laser l;
+struct mcl_model *mcl;
/**
* MCL test initialization
void testmcl_init()
{
/* MCL initialization */
- mcl.width = PLAYGROUND_WIDTH_M; /* in meters */
- mcl.height = PLAYGROUND_HEIGHT_M; /* in meters */
+ l.width = PLAYGROUND_WIDTH_M; /* in meters */
+ l.height = PLAYGROUND_HEIGHT_M; /* in meters */
/* the noises */
- mcl.gen_dnoise = 0.99;
- mcl.gen_anoise = 360;
- mcl.mov_dnoise = 0.003;
- mcl.mov_anoise = 0.03;
- mcl.w_min = 0.25;
- mcl.w_max = 2.0;
- mcl.eval_sigma = 160;
- mcl.aeval_sigma = 0.90;
- mcl.maxavdist = 0.150;
- /* bad cycles before reseting */
- mcl.maxnoisecycle = 10;
-
- /* amount of particles */
- mcl.count = 5000;
- mcl.parts = (struct mcl_particle *)
- malloc(sizeof(struct mcl_particle)*mcl.count);
-
- /* phases of the MCL */
- mcl.init = mcl_init;
- mcl.move = mcl_move;
- /* style of particles evaluation */
- mcl.update = mcl_update2;
- mcl.data = &meas_angles;
- mcl.beacon_cnt = BEACON_CNT;
- mcl.beacon_color = BEACON_BLUE;
-
- mcl.normalize = mcl_normalize;
- mcl.sort = mcl_partsort;
- mcl.resample = mcl_resample;
-
- /* cycles of enumeration */
- mcl.cycle = 0;
- mcl.noisecycle = 0;
-
- /* change flag */
- mcl.flag = MCL_RUN;
-
- /* generate particles with noises */
- mcl.init(&mcl);
+ l.pred_dnoise = 0.003;
+ l.pred_anoise = 0.03;
+ l.aeval_sigma = 0.90;
+
+ l.beacon_cnt = BEACON_CNT;
+ l.beacon_color = BEACON_BLUE;
+
+ /* Initialize with 5000 particles */
+ mcl = mcl_laser_init(&l, 5000);
}
int main(int argc, char *argv[])
struct timespec ts_start;
struct timespec ts_middle;
double t;
- double dx, dy, da;
+ struct mcl_laser_measurement meas_angles;
+ struct mcl_laser_state delta = {.x=0.001, .y=0.001, .angle=0};
+ struct mcl_laser_state *est_pos;
+ struct mcl_laser_state meas_pos;
+ mcl_thetas theta;
int i;
/* histogram parameters */
/* mcl init */
testmcl_init();
- parts = (struct mcl_particle *)mcl.parts;
-
- /* starting position */
- act_pos.x = 1.000; /* in meters */
- act_pos.y = 1.000; /* in meters */
-
- /* movement */
- dx = 0.001;
- dy = 0.001;
- da = 0.0;
meas_pos.x = 1.0;
meas_pos.y = 1.0;
meas_pos.angle = DEG2RAD(180);
- mcl_pos2ang(&meas_pos, theta, mcl.beacon_cnt, mcl.beacon_cnt);
- meas_angles.count = mcl.beacon_cnt;
- for(i=0; i<mcl.beacon_cnt; i++)
+ mcl_pos2ang(&meas_pos, theta, l.beacon_cnt, l.beacon_cnt);
+ meas_angles.count = l.beacon_cnt;
+ for(i=0; i<l.beacon_cnt; i++)
meas_angles.val[i] = theta[i];
-/* printf("meas: x=%g y=%g angle=%g w=%g th1=%g th2=%g th3=%g\n",
- meas_pos.x, meas_pos.y, meas_pos.angle,
- meas_pos.weight,
- meas_pos.theta[0], meas_pos.theta[1], meas_pos.theta[2]);*/
-/* for (i=0; i<mcl.count; i++) {
- printf("%d x=%g y=%g angle=%g w=%g th1=%g th2=%g th3=%g\n",
- i,
- parts[i].x, parts[i].y, parts[i].angle,
- parts[i].weight,
- parts[i].theta[0], parts[i].theta[1], parts[i].theta[2]);
- }*/
/* Main boucle */
for(i=1; i<TEST_COUNT; i++) {
/* time measurement */
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &ts_start);
- mcl.move(&mcl, dx, dy, da);
+ mcl->predict(mcl, &delta);
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &ts_middle);
- mcl.update(&mcl, mcl.data);
- mcl.normalize(&mcl);
- mcl.sort(&mcl);
- mcl.resample(&mcl);
+ mcl->update(mcl, &meas_angles);
+ mcl->resample(mcl);
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &ts_finish);
/* time measurement */
- est_pos = mcl_get_pos(&mcl);
+ est_pos = mcl->estimated;
t = (double)(ts_finish.tv_nsec - ts_start.tv_nsec) / 1000000000.0;
t += (ts_finish.tv_sec - ts_start.tv_sec);
projects / eurobot / public.git / commitdiff