Maltab now draws angle as color.
Always is used BEACON_CNT beacons, because current implementation depends
on the order of beacons and omitting one beacon is not possible.
VPATH = ../../motion
LIBPATH=$(PWD)/../../../build/linux/_compiled/lib
INCPATH=$(PWD)/../../../build/linux/_compiled/include
-CXXFLAGS += -I$(VPATH) -L$(LIBPATH) -I$(INCPATH)
+CXXFLAGS += -I$(VPATH) -L$(LIBPATH) -I$(INCPATH) -g
all: sf_posreg.mexglx sf_trgen.mexglx sf_mcl.mexglx
mex $(CXXFLAGS) $^
-sf_mcl.mexglx: sf_mcl.c $(LIBPATH)/libmcl.a $(LIBPATH)/librobomath.a # With fast library but printf doesn't work
-# sf_mcl.mexglx: sf_mcl.c ../mcl.c ../../robomath/robomath.c # For calling printf from MCL
+sf_mcl.mexglx: sf_mcl.c $(LIBPATH)/libmcl.a $(LIBPATH)/librobomath.a $(LIBPATH)/librobodim_eb2008.a # With fast library but printf doesn't work
+#sf_mcl.mexglx: sf_mcl.c ../mcl.c ../mcl_laser.c ../../robomath/robomath.c # For calling printf from MCL
mex $(CXXFLAGS) $^
NumRootInports 0
NumRootOutports 0
ParameterArgumentNames ""
- ComputedModelVersion "1.105"
+ ComputedModelVersion "1.129"
NumModelReferences 0
NumTestPointedSignals 0
}
SavedCharacterEncoding "UTF-8"
PostLoadFcn "init\nsim('mcl_test', 0)"
SaveDefaultBlockParams on
- SampleTimeColors off
+ SampleTimeColors on
LibraryLinkDisplay "none"
WideLines off
ShowLineDimensions on
ModifiedByFormat "%<Auto>"
LastModifiedBy "wsh"
ModifiedDateFormat "%<Auto>"
- LastModifiedDate "Tue Apr 29 11:52:05 2008"
- ModelVersionFormat "1.%<AutoIncrement:105>"
+ LastModifiedDate "Wed Apr 30 02:08:24 2008"
+ ModelVersionFormat "1.%<AutoIncrement:129>"
ConfigurationManager "None"
SimulationMode "normal"
LinearizationMsg "none"
}
System {
Name "mcl_test"
- Location [358, 561, 1340, 943]
+ Location [4, 618, 986, 1000]
Open on
ModelBrowserVisibility off
ModelBrowserWidth 200
System {
Name "Graphs"
Location [962, 152, 1482, 471]
- Open off
+ Open on
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
MaskIconOpaque on
MaskIconRotate "none"
MaskIconUnits "autoscale"
- MaskValueString "3000|0.01|2/180*pi|3/180*pi"
+ MaskValueString "3000|0.01|5/180*pi|1/180*pi"
MaskTabNameString ",,,"
}
Block {
Block {
BlockType Constant
Name "No odometry"
- Position [385, 89, 445, 111]
+ Position [395, 89, 455, 111]
NamePlacement "alternate"
Value "[0 0 0]"
- SampleTime "0.1"
+ SampleTime "-1"
}
Block {
BlockType SubSystem
Name "Pos to deltas"
Ports [1, 1]
- Position [240, 101, 335, 139]
+ Position [280, 101, 375, 139]
MinAlgLoopOccurrences off
RTWSystemCode "Auto"
FunctionWithSeparateData off
}
}
}
+ Block {
+ BlockType RateTransition
+ Name "Rate Transition"
+ Position [235, 98, 255, 142]
+ OutPortSampleTime "1/20"
+ }
Block {
BlockType SubSystem
Name "Rotating laser"
MaskIconOpaque on
MaskIconRotate "none"
MaskIconUnits "normalized"
- MaskValueString "1|0.01|on"
+ MaskValueString "3|0.01|on"
MaskTabNameString ",,"
System {
Name "Rotating laser"
- Location [127, 99, 1151, 684]
- Open on
+ Location [651, 66, 1675, 656]
+ Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
ScreenColor "white"
}
}
}
+ Block {
+ BlockType SubSystem
+ Name "Beacon memory3"
+ Ports [1, 1, 0, 1]
+ Position [510, 509, 565, 541]
+ TreatAsAtomicUnit on
+ MinAlgLoopOccurrences off
+ RTWSystemCode "Auto"
+ FunctionWithSeparateData off
+ MaskHideContents off
+ System {
+ Name "Beacon memory3"
+ Location [401, 346, 899, 646]
+ Open off
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "A4"
+ PaperUnits "centimeters"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "100"
+ Block {
+ BlockType Inport
+ Name "In1"
+ Position [110, 103, 140, 117]
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType TriggerPort
+ Name "Trigger"
+ Ports []
+ Position [225, 20, 245, 40]
+ StatesWhenEnabling "held"
+ }
+ Block {
+ BlockType Outport
+ Name "Out1"
+ Position [360, 103, 390, 117]
+ IconDisplay "Port number"
+ BusOutputAsStruct off
+ }
+ Line {
+ SrcBlock "In1"
+ SrcPort 1
+ DstBlock "Out1"
+ DstPort 1
+ }
+ }
+ }
Block {
BlockType Clock
Name "Clock"
- Position [740, 310, 760, 330]
+ Position [740, 320, 760, 340]
Decimation "10"
}
Block {
BlockType Reference
Name "Compare\nTo Constant"
Ports [1, 1]
- Position [50, 165, 100, 195]
+ Position [35, 164, 100, 196]
Orientation "left"
SourceBlock "simulink/Logic and Bit\nOperations/Compare"
"\nTo Constant"
BlockType Reference
Name "Compare\nTo Zero"
Ports [1, 1]
- Position [800, 305, 830, 335]
+ Position [800, 315, 830, 345]
SourceBlock "simulink/Logic and Bit\nOperations/Compare"
"\nTo Zero"
SourceType "Compare To Zero"
Block {
BlockType Demux
Name "Demux"
- Ports [1, 3]
- Position [344, 190, 396, 200]
+ Ports [1, 4]
+ Position [346, 190, 399, 200]
Orientation "down"
BackgroundColor "black"
NamePlacement "alternate"
ShowName off
- Outputs "3"
DisplayOption "bar"
}
Block {
BlockType Demux
Name "Demux1"
- Ports [1, 3]
- Position [414, 190, 466, 200]
+ Ports [1, 4]
+ Position [421, 190, 474, 200]
Orientation "down"
BackgroundColor "black"
NamePlacement "alternate"
ShowName off
- Outputs "3"
DisplayOption "bar"
}
Block {
Name "Do not produce\ndata at start. We\nhave to "
"wait one turn."
Ports [1, 1, 1]
- Position [825, 364, 925, 406]
+ Position [825, 374, 925, 416]
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
RTWSystemCode "Auto"
Orientation "left"
NamePlacement "alternate"
Variance "(noise_stdev/180*pi)^2"
- Seed "abs(floor(rand(3,1)*10000))"
+ Seed "abs(floor(rand(4,1)*10000))"
}
Block {
BlockType Mux
Name "Mux"
- Ports [3, 1]
- Position [700, 352, 705, 418]
+ Ports [4, 1]
+ Position [700, 354, 705, 436]
ShowName off
- Inputs "3"
DisplayOption "bar"
}
Block {
BlockType RateTransition
Name "Rate Transition"
- Position [750, 364, 790, 406]
+ Position [750, 374, 790, 416]
OutPortSampleTime "1/rps"
}
Block {
Ports [1, 1]
Position [585, 91, 625, 129]
IndexOptions "Index vector (dialog)"
- Indices "[1]"
+ Indices "[1 2 3 4]"
OutputSizes "1"
- InputPortWidth "3"
+ InputPortWidth "4"
}
Block {
BlockType Sum
OutDataTypeMode "Inherit via internal rule"
SaturateOnIntegerOverflow off
}
+ Block {
+ BlockType Sum
+ Name "Sum4"
+ Ports [2, 1]
+ Position [460, 515, 480, 535]
+ ShowName off
+ IconShape "round"
+ Inputs "++|"
+ CollapseMode "All dimensions"
+ InputSameDT off
+ OutDataTypeMode "Inherit via internal rule"
+ SaturateOnIntegerOverflow off
+ }
Block {
BlockType Sum
Name "Sum5"
Block {
BlockType Outport
Name "Simultaneous"
- Position [965, 378, 995, 392]
+ Position [965, 388, 995, 402]
Port "3"
IconDisplay "Port number"
BusOutputAsStruct off
Line {
SrcBlock "Demux1"
SrcPort 3
- Points [0, 15]
- Branch {
- DstBlock "Sum3"
- DstPort 1
- }
- Branch {
- Points [35, 0]
- DstBlock "Sum"
- DstPort 1
- }
+ DstBlock "Sum3"
+ DstPort 1
}
Line {
SrcBlock "Demux1"
DstPort 2
}
Branch {
+ Points [0, 15]
DstBlock "Demux1"
DstPort 1
}
Line {
SrcBlock "Demux"
SrcPort 3
- Points [0, 81; 150, 0]
+ Points [0, 81; 155, 0]
DstBlock "Beacon memory"
DstPort trigger
}
SrcPort 1
Points [15, 0]
Branch {
+ Points [0, 65]
DstBlock "Demux"
DstPort 1
}
}
Branch {
Points [0, 70]
- DstBlock "Sum3"
- DstPort 2
+ Branch {
+ DstBlock "Sum3"
+ DstPort 2
+ }
+ Branch {
+ Points [0, 70]
+ DstBlock "Sum4"
+ DstPort 2
+ }
}
}
}
Line {
SrcBlock "Compare\nTo Constant"
SrcPort 1
- Points [-30, 0; 0, 110]
+ Points [-15, 0; 0, 110]
DstBlock "Integrator"
DstPort 2
}
DstBlock "Individual output"
DstPort trigger
}
+ Line {
+ SrcBlock "Sum4"
+ SrcPort 1
+ DstBlock "Beacon memory3"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Demux1"
+ SrcPort 4
+ Points [0, 10]
+ Branch {
+ Points [20, 0]
+ DstBlock "Sum"
+ DstPort 1
+ }
+ Branch {
+ DstBlock "Sum4"
+ DstPort 1
+ }
+ }
+ Line {
+ SrcBlock "Beacon memory3"
+ SrcPort 1
+ Points [55, 0; 0, -100]
+ DstBlock "Mux"
+ DstPort 4
+ }
}
}
Block {
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
low "0"
- gain "232.844"
+ gain "360"
high "360"
}
Block {
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
low "0"
- gain "1.47"
+ gain "0.7156"
high "3"
}
Block {
RTWMemSecDataConstants "Inherit from model"
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
- sw "0"
+ sw "1"
action "0"
varsize off
}
Labels [0, 0]
SrcBlock "Switch"
SrcPort 1
- Points [0, 0]
+ Points [5, 0]
Branch {
DstBlock "MCL"
DstPort 2
SrcBlock "Mux"
SrcPort 1
Points [10, 0]
- Branch {
- DstBlock "Pos to deltas"
- DstPort 1
- }
Branch {
Points [0, -60; 530, 0]
Branch {
DstBlock "Rotating laser"
DstPort 1
}
+ Branch {
+ DstBlock "Rate Transition"
+ DstPort 1
+ }
}
Line {
SrcBlock "to rad"
DstBlock "Switch"
DstPort 2
}
+ Line {
+ SrcBlock "Rate Transition"
+ SrcPort 1
+ DstBlock "Pos to deltas"
+ DstPort 1
+ }
}
}
NumRootInports 0
NumRootOutports 0
ParameterArgumentNames ""
- ComputedModelVersion "1.326"
+ ComputedModelVersion "1.327"
NumModelReferences 0
NumTestPointedSignals 0
}
ModifiedByFormat "%<Auto>"
LastModifiedBy "wsh"
ModifiedDateFormat "%<Auto>"
- LastModifiedDate "Tue Apr 29 11:44:26 2008"
- ModelVersionFormat "1.%<AutoIncrement:326>"
+ LastModifiedDate "Wed Apr 30 02:09:53 2008"
+ ModelVersionFormat "1.%<AutoIncrement:327>"
ConfigurationManager "None"
SimulationMode "normal"
LinearizationMsg "none"
}
System {
Name "reg_mcl"
- Location [391, 580, 1246, 1057]
+ Location [666, 262, 1521, 739]
Open on
ModelBrowserVisibility off
ModelBrowserWidth 200
MaskIconOpaque on
MaskIconRotate "none"
MaskIconUnits "normalized"
- MaskValueString "3|1|on"
+ MaskValueString "3|0.01|on"
MaskTabNameString ",,"
System {
Name "Rotating laser"
- Location [84, 67, 1108, 652]
+ Location [651, 66, 1675, 656]
Open off
ModelBrowserVisibility off
ModelBrowserWidth 200
}
}
}
+ Block {
+ BlockType SubSystem
+ Name "Beacon memory3"
+ Ports [1, 1, 0, 1]
+ Position [510, 509, 565, 541]
+ TreatAsAtomicUnit on
+ MinAlgLoopOccurrences off
+ RTWSystemCode "Auto"
+ FunctionWithSeparateData off
+ MaskHideContents off
+ System {
+ Name "Beacon memory3"
+ Location [401, 346, 899, 646]
+ Open off
+ ModelBrowserVisibility off
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "A4"
+ PaperUnits "centimeters"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "100"
+ Block {
+ BlockType Inport
+ Name "In1"
+ Position [110, 103, 140, 117]
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType TriggerPort
+ Name "Trigger"
+ Ports []
+ Position [225, 20, 245, 40]
+ StatesWhenEnabling "held"
+ }
+ Block {
+ BlockType Outport
+ Name "Out1"
+ Position [360, 103, 390, 117]
+ IconDisplay "Port number"
+ BusOutputAsStruct off
+ }
+ Line {
+ SrcBlock "In1"
+ SrcPort 1
+ DstBlock "Out1"
+ DstPort 1
+ }
+ }
+ }
Block {
BlockType Clock
Name "Clock"
- Position [740, 310, 760, 330]
+ Position [740, 320, 760, 340]
Decimation "10"
}
Block {
BlockType Reference
Name "Compare\nTo Constant"
Ports [1, 1]
- Position [50, 165, 100, 195]
+ Position [35, 164, 100, 196]
Orientation "left"
SourceBlock "simulink/Logic and Bit\nOperations/Compare"
"\nTo Constant"
BlockType Reference
Name "Compare\nTo Zero"
Ports [1, 1]
- Position [800, 305, 830, 335]
+ Position [800, 315, 830, 345]
SourceBlock "simulink/Logic and Bit\nOperations/Compare"
"\nTo Zero"
SourceType "Compare To Zero"
Block {
BlockType Demux
Name "Demux"
- Ports [1, 3]
- Position [344, 190, 396, 200]
+ Ports [1, 4]
+ Position [346, 190, 399, 200]
Orientation "down"
BackgroundColor "black"
NamePlacement "alternate"
ShowName off
- Outputs "3"
DisplayOption "bar"
}
Block {
BlockType Demux
Name "Demux1"
- Ports [1, 3]
- Position [414, 190, 466, 200]
+ Ports [1, 4]
+ Position [421, 190, 474, 200]
Orientation "down"
BackgroundColor "black"
NamePlacement "alternate"
ShowName off
- Outputs "3"
DisplayOption "bar"
}
Block {
Name "Do not produce\ndata at start. We\nhave to "
"wait one turn."
Ports [1, 1, 1]
- Position [825, 364, 925, 406]
+ Position [825, 374, 925, 416]
TreatAsAtomicUnit on
MinAlgLoopOccurrences off
RTWSystemCode "Auto"
Orientation "left"
NamePlacement "alternate"
Variance "(noise_stdev/180*pi)^2"
- Seed "abs(floor(rand(3,1)*10000))"
+ Seed "abs(floor(rand(4,1)*10000))"
}
Block {
BlockType Mux
Name "Mux"
- Ports [3, 1]
- Position [700, 352, 705, 418]
+ Ports [4, 1]
+ Position [700, 354, 705, 436]
ShowName off
- Inputs "3"
DisplayOption "bar"
}
Block {
BlockType RateTransition
Name "Rate Transition"
- Position [750, 364, 790, 406]
+ Position [750, 374, 790, 416]
OutPortSampleTime "1/rps"
}
Block {
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 2 3 4]"
+ OutputSizes "1"
+ InputPortWidth "4"
+ }
Block {
BlockType Sum
Name "Subtract"
OutDataTypeMode "Inherit via internal rule"
SaturateOnIntegerOverflow off
}
+ Block {
+ BlockType Sum
+ Name "Sum4"
+ Ports [2, 1]
+ Position [460, 515, 480, 535]
+ ShowName off
+ IconShape "round"
+ Inputs "++|"
+ CollapseMode "All dimensions"
+ InputSameDT off
+ OutDataTypeMode "Inherit via internal rule"
+ SaturateOnIntegerOverflow off
+ }
Block {
BlockType Sum
Name "Sum5"
Block {
BlockType Outport
Name "Simultaneous"
- Position [965, 378, 995, 392]
+ Position [965, 388, 995, 402]
Port "3"
IconDisplay "Port number"
BusOutputAsStruct off
}
+ Line {
+ SrcBlock "Beacon memory3"
+ SrcPort 1
+ Points [55, 0; 0, -100]
+ DstBlock "Mux"
+ DstPort 4
+ }
+ Line {
+ SrcBlock "Demux1"
+ SrcPort 4
+ Points [0, 10]
+ Branch {
+ DstBlock "Sum4"
+ DstPort 1
+ }
+ Branch {
+ Points [20, 0]
+ DstBlock "Sum"
+ DstPort 1
+ }
+ }
+ Line {
+ SrcBlock "Sum4"
+ SrcPort 1
+ DstBlock "Beacon memory3"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Selector"
+ SrcPort 1
+ Points [140, 0]
+ DstBlock "Individual output"
+ DstPort trigger
+ }
+ Line {
+ SrcBlock "Beacon memory1"
+ SrcPort 1
+ DstBlock "Mux"
+ DstPort 2
+ }
Line {
SrcBlock "Integrator"
SrcPort state
Line {
SrcBlock "Compare\nTo Constant"
SrcPort 1
- Points [-30, 0; 0, 110]
+ Points [-15, 0; 0, 110]
DstBlock "Integrator"
DstPort 2
}
Points [0, 70]
Branch {
Points [0, 70]
- DstBlock "Sum3"
- DstPort 2
+ Branch {
+ Points [0, 70]
+ DstBlock "Sum4"
+ DstPort 2
+ }
+ Branch {
+ DstBlock "Sum3"
+ DstPort 2
+ }
}
Branch {
DstBlock "Sum2"
SrcPort 1
Points [15, 0]
Branch {
- DstBlock "Demux"
+ DstBlock "Selector"
DstPort 1
}
Branch {
- Points [400, 0]
- DstBlock "Individual output"
- DstPort trigger
+ Points [0, 65]
+ DstBlock "Demux"
+ DstPort 1
}
}
Line {
DstBlock "Mux"
DstPort 1
}
- Line {
- SrcBlock "Beacon memory1"
- SrcPort 1
- DstBlock "Mux"
- DstPort 2
- }
Line {
SrcBlock "Beacon memory2"
SrcPort 1
Line {
SrcBlock "Demux"
SrcPort 3
- Points [0, 81; 150, 0]
+ Points [0, 81; 155, 0]
DstBlock "Beacon memory"
DstPort trigger
}
SrcPort 1
Points [-30, 0]
Branch {
+ Points [0, 15]
DstBlock "Demux1"
DstPort 1
}
Line {
SrcBlock "Demux1"
SrcPort 3
- Points [0, 15]
- Branch {
- Points [35, 0]
- DstBlock "Sum"
- DstPort 1
- }
- Branch {
- DstBlock "Sum3"
- DstPort 1
- }
+ DstBlock "Sum3"
+ DstPort 1
}
Line {
SrcBlock "Sum5"
DstBlock "Rate Transition2"
DstPort 1
}
- Branch {
- DstBlock "Rotating laser"
- DstPort 1
- }
Branch {
Labels [2, 0]
Points [0, -45; 300, 0]
DstPort 1
}
}
+ Branch {
+ DstBlock "Rotating laser"
+ DstPort 1
+ }
}
Line {
SrcBlock "Clock"
#define BITMAP_WIDTH 300
#define BITMAP_HEIGHT 210
+#define BITMAP_R (0*BITMAP_WIDTH*BITMAP_HEIGHT)
+#define BITMAP_G (1*BITMAP_WIDTH*BITMAP_HEIGHT)
+#define BITMAP_B (2*BITMAP_WIDTH*BITMAP_HEIGHT)
#define PARAM_PART_COUNT_IDX 0
#define PARAM_NOISE_XY_IDX 1
if (!ssSetNumOutputPorts(S, 2)) return;
ssSetOutputPortWidth(S, OUTPUT_EST_POS, 3);
- ssSetOutputPortMatrixDimensions(S, OUTPUT_BITMAP, BITMAP_HEIGHT, BITMAP_WIDTH);
+ ssSetOutputPortDataType(S, OUTPUT_BITMAP, SS_UINT8);
+ ssAllowSignalsWithMoreThan2D(S);
+ int dims[3] = { BITMAP_HEIGHT, BITMAP_WIDTH, 3 };
+ DimsInfo_T dimsInfo = { .width = 3*BITMAP_HEIGHT*BITMAP_WIDTH,
+ .numDims = 3,
+ .dims = dims};
+ ssSetOutputPortDimensionInfo(S, OUTPUT_BITMAP, &dimsInfo);
+ /* ssSetOutputPortMatrixDimensions(S, OUTPUT_BITMAP, BITMAP_HEIGHT, BITMAP_WIDTH); */
ssSetNumSampleTimes(S, PORT_BASED_SAMPLE_TIMES);
ssSetNumRWork(S, 0);
pwork[1] = (void*)last_angles;
/* MCL initialization */
- l->beacon_cnt = 3;
l->width = 3.0; /* in m */
l->height = 2.1; /* in m */
/* the noises */
l->pred_dnoise = NOISE_XY(S);
l->pred_anoise = NOISE_ANGLE(S);
l->aeval_sigma = AEVAL_SIGMA(S);
+ l->beacon_miss = 0.0;
+ l->false_beacon = 0.0;
+
mcl_laser_init(l, PART_COUNT(S));
}
}
+static void hsv2rgb(uint8_t rgb[3], float hsv[3])
+{
+ float h=hsv[0], s=hsv[1], v=hsv[2];
+ int i = (int)floor(h*6) % 6;
+ float f = h*6-floor(h*6);
+ uint8_t t = v*255 * (1-s);
+ uint8_t n = v*255 * (1-s*f);
+ uint8_t p = v*255 * (1 - (s*(1-f)));
+ uint8_t e = v*255;
+ switch (i) {
+ case 0: { uint8_t o[3] = {e,p,t}; memcpy(rgb, o, sizeof(o)); break; }
+ case 1: { uint8_t o[3] = {n,e,t}; memcpy(rgb, o, sizeof(o)); break; }
+ case 2: { uint8_t o[3] = {t,e,p}; memcpy(rgb, o, sizeof(o)); break; }
+ case 3: { uint8_t o[3] = {t,n,e}; memcpy(rgb, o, sizeof(o)); break; }
+ case 4: { uint8_t o[3] = {p,t,e}; memcpy(rgb, o, sizeof(o)); break; }
+ case 5: { uint8_t o[3] = {e,t,n}; memcpy(rgb, o, sizeof(o)); break; }
+ }
+}
+
/* Function: mdlOutputs =======================================================
* Abstract:
* In this function, you compute the outputs of your S-function
struct mcl_laser_state *parts = l->parts;
double *pos = ssGetOutputPortRealSignal(S,OUTPUT_EST_POS);
- double *bitmap = ssGetOutputPortRealSignal(S,OUTPUT_BITMAP);
+ uint8_t *bitmap = ssGetOutputPortSignal(S,OUTPUT_BITMAP);
InputRealPtrsType iPtrsMove = ssGetInputPortRealSignalPtrs(S, INPUT_MOVE);
InputRealPtrsType iPtrsMeasure = ssGetInputPortRealSignalPtrs(S, INPUT_MEASURE);
int_T iMeasureWidth = ssGetInputPortWidth(S,INPUT_MEASURE);
/* for (i=0; i<angles.count; i++) printf(" a=%5.0f deg", angles.val[i]/M_PI*180); */
/* printf("\n"); */
+ struct mcl_laser_state *parts = l->parts;
mcl->update(mcl, &angles);
mcl->resample(mcl);
+ parts = l->parts;
+/* for (i=0; i < l->mcl.count; i++) { */
+/* printf("%3d: x=%4.2f y=%4.2g a=%4.2g b=%d w=%5.3g\n", */
+/* i, parts[i].x, parts[i].y, parts[i].angle, parts[i].beacon, mcl->weight[i]); */
+/* } */
+ /* ssSetStopRequested(S, 1); */
}
*last_angles = angles;
}
/* Update probability bitmap */
- for (i=0; i<BITMAP_WIDTH*BITMAP_HEIGHT; i++)
+ /* parts = l->resampled; */
+ parts = l->parts;;
+ for (i=0; i<BITMAP_WIDTH*BITMAP_HEIGHT*3; 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;
+ int x, y, pixel;
+ double color = 1;
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*log(1+mcl->weight[i])/log(1+max);
- color = fmax(color, 16);
- bitmap[x*BITMAP_HEIGHT+y] = color;
+ #if 0
+ color = log(1+mcl->weight[i])/log(1+max);
+ /* color = 0.3+0*mcl->weight[i]/max; */
+ color = fmax(color, 0.2);
+ #endif
+ uint8_t rgb[3];
+ float hsv[3] = {fmod(parts[i].angle, 2*M_PI)/2/M_PI, 1, color};
+ hsv2rgb(rgb, hsv);
+ pixel = x*BITMAP_HEIGHT+y;
+ bitmap[BITMAP_R+pixel] = rgb[0];
+ bitmap[BITMAP_G+pixel] = rgb[1];
+ bitmap[BITMAP_B+pixel] = rgb[2];
}
}
block.SetSimViewingDevice(1);\r
\r
block.SetAccelRunOnTLC(false);\r
+ block.AllowSignalsWithMoreThan2D=true;\r
\r
block.RegBlockMethod('PostPropagationSetup', @DoPostPropSetup);\r
block.RegBlockMethod('ProcessParameters', @ProcessPrms);\r
{
mcl->count = count;
mcl->weight = malloc(count*sizeof(*mcl->weight));
+ memset(mcl->weight, 0, count*sizeof(*mcl->weight));
mcl->estimated = estimated;
}
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
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);
+ parts[i].beacon = rand() % BEACON_CNT;
}
}
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);*/
+ if (drand48() < l->beacon_miss) parts[i].beacon++;
+ if (drand48() < l->false_beacon) parts[i].beacon--;
+ if (parts[i].beacon >= BEACON_CNT)
+ parts[i].beacon = 0;
+ if (parts[i].beacon < 0)
+ parts[i].beacon = BEACON_CNT-1;
+
+/* printf("%3d: x=%g y=%g a=%g b=%d\n", */
+/* i, parts[i].x, parts[i].y, parts[i].angle, parts[i].beacon); */
}
/* Update the last estimated position without noise */
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;
+ 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 theta;
double p;
double diff;
- int i, it, im;
+ int i, 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] = 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);
- }
+ theta = mcl_beacon_angle(&parts[i], l->beacon_color);
+ diff = fabs(theta - angles->val[im]);
+ if (diff > M_PI) diff = 2*M_PI-diff;
+ p = gaussian(diff, l->aeval_sigma);
mcl->weight[i] *= p;
+
+ parts[i].beacon++;
+ parts[i].beacon %= BEACON_CNT;
}
if (mcl->weight[i] > wmax) {
wmax = mcl->weight[i];
* also save one pass as compared to standalone
* mcl_normalize(). */
sum = Q[mcl->count];
- for (i = 1; i <= mcl->count; i++)
+ for (i = 0; i <= mcl->count; i++)
Q[i] /= sum;
/* We do not store the last element of T since it is always
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;
return mcl;
}
+double mcl_beacon_angle(const struct mcl_laser_state *part,
+ unsigned char color)
+{
+ double head, theta;
+ int i;
+ const struct beacon_pos *beacon;
+
+ if (color == BEACON_RED) {
+ head = part->angle + M_PI;
+ beacon = beacon_red;
+ } else {
+ head = part->angle;
+ beacon = beacon_blue;
+ }
+
+ head = (head < 0) ? 2*M_PI + head : head;
+
+
+ i = part->beacon;
+ theta = atan2(beacon[i].y-part->y, beacon[i].x-part->x);
+ /* normalize angles */
+ if (theta < 0) theta = 2*M_PI + theta;
+ theta = (theta-head < 0) ?
+ 2*M_PI+theta-head : theta-head;
+ return theta;
+}
+
/**
* Convert the position to angles between robot's head and reflectors
* (angles sense is counter-clockwise)
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;
+ struct mcl_laser_state p = *part;
+ for (i=0; i<BEACON_CNT; i++) {
+ p.beacon = i;
+ theta[i] = mcl_beacon_angle(&p, color);
}
-
- /* sort and store results */
- qsort(theta, cnt, sizeof(double), cmpd);
}
#include <mcl.h>
#include <robodim_eb2008.h>
+#include <stdint.h>
#define MCL_LASER_MAX_MEASURE 10
struct mcl_laser_state {
- double x;
- double y;
- double angle;
+ float x;
+ float y;
+ float angle;
+ int8_t beacon; /* from which beacon we expect next measure */
};
struct mcl_laser_measurement {
/* noises: movement noise, */
double pred_dnoise; /* prediction distance noises */
double pred_anoise; /* prediction angle noises */
+ double beacon_miss; /* beacon miss probability */
+ double false_beacon; /* false beacon probability */
/* used to evaluate the particles */
double aeval_sigma; /* sigma used for angles evaluating */
/* size of the playground (in meters) */
/* measured data */
void *data;
#endif
- unsigned char beacon_cnt; /* number of beacons */
unsigned char beacon_color; /* red/blue beacons */
/* Temporary arrays for resampling */
struct mcl_model *mcl_laser_init(struct mcl_laser *l, unsigned count);
-
+double mcl_beacon_angle(const struct mcl_laser_state *part,
+ unsigned char color);
void mcl_pos2ang(const struct mcl_laser_state *part,
mcl_thetas theta,
unsigned char cnt, unsigned char color);
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);
+ mcl = mcl_laser_init(&l, 5);
}
int main(int argc, char *argv[])
struct mcl_laser_state *est_pos;
struct mcl_laser_state meas_pos;
mcl_thetas theta;
- int i;
+ int i, beacon;
/* histogram parameters */
hist_move.count = HIST_COUNT;
testmcl_init();
meas_pos.x = 1.0;
- meas_pos.y = 1.0;
+ meas_pos.y = 1.05;
meas_pos.angle = DEG2RAD(180);
- 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_pos.beacon = 0;
+ mcl_pos2ang(&meas_pos, theta, 0, BEACON_BLUE);
+ meas_angles.count = BEACON_CNT;
+ for(i=0; i<BEACON_CNT; i++)
meas_angles.val[i] = theta[i];
-
-
+ l.parts[0] = meas_pos;
+ beacon = 0;
/* Main boucle */
for(i=1; i<TEST_COUNT; i++) {
+ meas_angles.count = 1;
+ meas_angles.val[0] = theta[beacon++];
+ beacon %= BEACON_CNT;
+
/* time measurement */
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &ts_start);
mcl->predict(mcl, &delta);
projects / eurobot / public.git / commitdiff