NumRootInports 0
NumRootOutports 0
ParameterArgumentNames ""
- ComputedModelVersion "1.81"
+ ComputedModelVersion "1.91"
NumModelReferences 0
NumTestPointedSignals 0
}
SavedCharacterEncoding "UTF-8"
PostLoadFcn "init\nsim('mcl_test', 0)"
SaveDefaultBlockParams on
- SampleTimeColors on
+ SampleTimeColors off
LibraryLinkDisplay "none"
WideLines off
ShowLineDimensions on
StatusBar on
BrowserShowLibraryLinks off
BrowserLookUnderMasks off
- InitFcn "init"
+ InitFcn "init;"
Created "Fri Apr 18 14:57:12 2008"
Creator "andrei"
UpdateHistory "UpdateHistoryNever"
ModifiedByFormat "%<Auto>"
LastModifiedBy "wsh"
ModifiedDateFormat "%<Auto>"
- LastModifiedDate "Sun Apr 20 13:53:01 2008"
- ModelVersionFormat "1.%<AutoIncrement:81>"
+ LastModifiedDate "Sun Apr 20 16:01:02 2008"
+ ModelVersionFormat "1.%<AutoIncrement:91>"
ConfigurationManager "None"
SimulationMode "normal"
LinearizationMsg "none"
ShowPageBoundaries off
ZoomFactor "100"
ReportName "simulink-default.rpt"
- Block {
- BlockType Scope
- Name "Angle"
- Ports [1]
- Position [870, 89, 900, 121]
- Floating off
- Location [498, 791, 1148, 973]
- Open on
- NumInputPorts "1"
- List {
- ListType AxesTitles
- axes1 "%<SignalLabel>"
- }
- TimeRange "100"
- YMin "0"
- YMax "7"
- SaveName "ScopeData1"
- DataFormat "StructureWithTime"
- SampleTime "0"
- }
Block {
BlockType "M-S-Function"
Name "Draw robot"
Ports [2]
- Position [835, 221, 930, 259]
+ Position [825, 266, 920, 304]
+ ForegroundColor "red"
+ BackgroundColor "cyan"
FunctionName "sfdrawrobot"
Parameters "desc,0"
}
BlockType "M-S-Function"
Name "Draw robot1"
Ports [2]
- Position [835, 281, 930, 319]
+ Position [825, 51, 920, 89]
+ ForegroundColor "blue"
FunctionName "sfdrawrobot"
Parameters "desc,'b-'"
}
+ Block {
+ BlockType SubSystem
+ Name "Graphs"
+ Ports [2]
+ Position [825, 130, 925, 190]
+ MinAlgLoopOccurrences off
+ RTWSystemCode "Auto"
+ FunctionWithSeparateData off
+ MaskHideContents off
+ System {
+ Name "Graphs"
+ Location [962, 152, 1482, 471]
+ 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 "Pos"
+ Position [25, 110, 55, 124]
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Inport
+ Name "Est pos"
+ Position [25, 223, 55, 237]
+ Port "2"
+ IconDisplay "Port number"
+ }
+ Block {
+ BlockType Scope
+ Name "Angle"
+ Ports [1]
+ Position [410, 109, 440, 141]
+ Floating off
+ Location [475, 726, 1126, 908]
+ Open on
+ NumInputPorts "1"
+ List {
+ ListType AxesTitles
+ axes1 "%<SignalLabel>"
+ }
+ TimeRange "100"
+ YMin "0"
+ YMax "360"
+ SaveName "ScopeData1"
+ DataFormat "StructureWithTime"
+ SampleTime "0"
+ }
+ Block {
+ BlockType Gain
+ Name "Gain"
+ Position [295, 109, 330, 141]
+ Gain "180/pi"
+ ParameterDataTypeMode "Inherit via internal rule"
+ OutDataTypeMode "Inherit via internal rule"
+ SaturateOnIntegerOverflow off
+ Port {
+ PortNumber 1
+ Name "Angle [deg]"
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
+ }
+ Block {
+ BlockType Mux
+ Name "Mux"
+ Ports [2, 1]
+ Position [250, 46, 255, 84]
+ ShowName off
+ Inputs "2"
+ DisplayOption "bar"
+ Port {
+ PortNumber 1
+ Name "Position [m]"
+ PropagatedSignals ", , "
+ RTWStorageClass "Auto"
+ DataLoggingNameMode "SignalName"
+ }
+ }
+ Block {
+ BlockType Mux
+ Name "Mux1"
+ Ports [2, 1]
+ Position [250, 106, 255, 144]
+ ShowName off
+ Inputs "2"
+ DisplayOption "bar"
+ }
+ Block {
+ BlockType Selector
+ Name "Selector"
+ Ports [1, 1]
+ Position [110, 36, 150, 74]
+ IndexOptions "Index vector (dialog)"
+ Indices "[1 2]"
+ OutputSizes "1"
+ InputPortWidth "3"
+ }
+ Block {
+ BlockType Selector
+ Name "Selector1"
+ Ports [1, 1]
+ Position [110, 96, 150, 134]
+ IndexOptions "Index vector (dialog)"
+ Indices "3"
+ OutputSizes "1"
+ InputPortWidth "3"
+ }
+ Block {
+ BlockType Selector
+ Name "Selector2"
+ Ports [1, 1]
+ Position [110, 236, 150, 274]
+ IndexOptions "Index vector (dialog)"
+ Indices "3"
+ OutputSizes "1"
+ InputPortWidth "3"
+ }
+ Block {
+ BlockType Selector
+ Name "Selector3"
+ Ports [1, 1]
+ Position [110, 171, 150, 209]
+ IndexOptions "Index vector (dialog)"
+ Indices "[1 2]"
+ OutputSizes "1"
+ InputPortWidth "3"
+ }
+ Block {
+ BlockType Scope
+ Name "XY"
+ Ports [1]
+ Position [370, 49, 400, 81]
+ Floating off
+ Location [469, 497, 1123, 665]
+ Open on
+ NumInputPorts "1"
+ ZoomMode "xonly"
+ List {
+ ListType AxesTitles
+ axes1 "%<SignalLabel>"
+ }
+ TimeRange "100"
+ YMin "0"
+ YMax "3"
+ DataFormat "StructureWithTime"
+ SampleTime "0"
+ }
+ Line {
+ SrcBlock "Pos"
+ SrcPort 1
+ Points [15, 0]
+ Branch {
+ Points [0, -60]
+ DstBlock "Selector"
+ DstPort 1
+ }
+ Branch {
+ DstBlock "Selector1"
+ DstPort 1
+ }
+ }
+ Line {
+ SrcBlock "Est pos"
+ SrcPort 1
+ Points [0, -15; 15, 0]
+ Branch {
+ Points [0, -25]
+ DstBlock "Selector3"
+ DstPort 1
+ }
+ Branch {
+ Points [0, 40]
+ DstBlock "Selector2"
+ DstPort 1
+ }
+ }
+ Line {
+ SrcBlock "Selector"
+ SrcPort 1
+ DstBlock "Mux"
+ DstPort 1
+ }
+ Line {
+ Name "Position [m]"
+ Labels [0, 0]
+ SrcBlock "Mux"
+ SrcPort 1
+ DstBlock "XY"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Selector3"
+ SrcPort 1
+ Points [40, 0; 0, -115]
+ DstBlock "Mux"
+ DstPort 2
+ }
+ Line {
+ SrcBlock "Mux1"
+ SrcPort 1
+ DstBlock "Gain"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Selector1"
+ SrcPort 1
+ DstBlock "Mux1"
+ DstPort 1
+ }
+ Line {
+ SrcBlock "Selector2"
+ SrcPort 1
+ Points [80, 0]
+ DstBlock "Mux1"
+ DstPort 2
+ }
+ Line {
+ Name "Angle [deg]"
+ Labels [0, 0]
+ SrcBlock "Gain"
+ SrcPort 1
+ DstBlock "Angle"
+ DstPort 1
+ }
+ }
+ }
Block {
BlockType Ground
Name "Ground"
- Position [745, 300, 765, 320]
+ Position [770, 70, 790, 90]
}
Block {
BlockType "S-Function"
Name "MCL"
Ports [2, 2]
- Position [535, 88, 715, 157]
+ Position [535, 158, 715, 227]
+ BackgroundColor "green"
+ ShowName off
FunctionName "sf_mcl"
- Parameters "100"
- MaskPromptString "Number of samples"
- MaskStyleString "edit"
- MaskTunableValueString "on"
- MaskEnableString "on"
- MaskVisibilityString "on"
- MaskToolTipString "on"
+ Parameters "count, noise_xy, noise_angle, aeval_sigma"
+ MaskPromptString "Number of samples|Move noise X,Y|Move noise ang"
+"le|Measure dispersion"
+ MaskStyleString "edit,edit,edit,edit"
+ MaskTunableValueString "off,on,on,on"
+ MaskCallbackString "|||"
+ MaskEnableString "on,on,on,on"
+ MaskVisibilityString "on,on,on,on"
+ MaskToolTipString "on,on,on,on"
+ MaskVarAliasString ",,,"
+ MaskVariables "count=@1;noise_xy=@2;noise_angle=@3;aeval_sigma"
+"=@4;"
MaskDisplay "port_label('input', 1, '(dx,dy,da)')\nport_labe"
"l('input', 2, '(th1,th2,th3)')\nport_label('output', 1, 'est_pos=(x,y,a)')\np"
-"ort_label('output', 2, 'particle bitmap')\n"
+"ort_label('output', 2, 'particle bitmap')\ndisp('MCL')"
MaskIconFrame on
MaskIconOpaque on
MaskIconRotate "none"
MaskIconUnits "autoscale"
- MaskValueString "0"
+ MaskValueString "3000|0.01|2/180*pi|2/180*pi"
+ MaskTabNameString ",,,"
}
Block {
BlockType SubSystem
Name "Measurement"
Ports [1, 1]
- Position [310, 132, 385, 168]
+ Position [310, 202, 385, 238]
MinAlgLoopOccurrences off
RTWSystemCode "Auto"
FunctionWithSeparateData off
BlockType Reference
Name "Measurement noise\nstd. deviation [deg]"
Ports [1, 1]
- Position [394, 200, 446, 220]
+ Position [394, 270, 446, 290]
Orientation "up"
NamePlacement "alternate"
SourceBlock "simulink/Math\nOperations/Slider\nGain"
BlockType Mux
Name "Mux"
Ports [3, 1]
- Position [190, 131, 195, 169]
+ Position [190, 101, 195, 139]
ShowName off
Inputs "3"
DisplayOption "bar"
Block {
BlockType Constant
Name "No laser"
- Position [370, 94, 430, 116]
+ Position [370, 164, 430, 186]
NamePlacement "alternate"
Value "[0 0 0]"
SampleTime "-1"
Block {
BlockType Constant
Name "No odometry"
- Position [385, 19, 445, 41]
+ Position [385, 89, 445, 111]
NamePlacement "alternate"
Value "[0 0 0]"
SampleTime "0.1"
Block {
BlockType RandomNumber
Name "Random\nNumber"
- Position [405, 240, 435, 270]
+ Position [405, 310, 435, 340]
Orientation "up"
NamePlacement "alternate"
Variance "(1/180*pi)^2"
Block {
BlockType RateTransition
Name "Rate Transition"
- Position [240, 129, 280, 171]
+ Position [240, 199, 280, 241]
OutPortSampleTime "1"
}
- Block {
- BlockType Selector
- Name "Selector"
- Ports [1, 1]
- Position [795, 26, 835, 64]
- IndexOptions "Index vector (dialog)"
- Indices "[1 2]"
- OutputSizes "1"
- InputPortWidth "3"
- }
- Block {
- BlockType Selector
- Name "Selector1"
- Ports [1, 1]
- Position [795, 86, 835, 124]
- IndexOptions "Index vector (dialog)"
- Indices "3"
- OutputSizes "1"
- InputPortWidth "3"
- }
Block {
BlockType Reference
Name "Slider\nGain ANGLE"
Ports [1, 1]
- Position [75, 210, 125, 240]
+ Position [75, 180, 125, 210]
SourceBlock "simulink/Math\nOperations/Slider\nGain"
SourceType "Slider Gain"
ShowPortLabels "FromPortIcon"
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
low "0"
- gain "1.5527"
- high "6.28"
+ gain "90"
+ high "360"
}
Block {
BlockType Reference
Name "Slider\nGain X"
Ports [1, 1]
- Position [80, 64, 125, 96]
+ Position [80, 34, 125, 66]
SourceBlock "simulink/Math\nOperations/Slider\nGain"
SourceType "Slider Gain"
ShowPortLabels "FromPortIcon"
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
low "0"
- gain "1.2"
+ gain "1"
high "3"
}
Block {
BlockType Reference
Name "Slider\nGain Y"
Ports [1, 1]
- Position [75, 135, 125, 165]
+ Position [75, 105, 125, 135]
SourceBlock "simulink/Math\nOperations/Slider\nGain"
SourceType "Slider Gain"
ShowPortLabels "FromPortIcon"
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
low "0"
- gain "1"
+ gain "0.5"
high "2.1"
}
Block {
BlockType SubSystem
Name "Subsystem"
Ports [1, 1]
- Position [240, 31, 335, 69]
+ Position [240, 101, 335, 139]
MinAlgLoopOccurrences off
RTWSystemCode "Auto"
FunctionWithSeparateData off
BlockType Sum
Name "Sum"
Ports [2, 1]
- Position [410, 140, 430, 160]
+ Position [410, 210, 430, 230]
ShowName off
IconShape "round"
Inputs "|++"
BlockType Reference
Name "Switch"
Ports [2, 1]
- Position [470, 122, 500, 158]
+ Position [470, 192, 500, 228]
SourceBlock "simulink/Signal\nRouting/Manual Switch"
SourceType "Manual Switch"
ShowPortLabels "FromPortIcon"
BlockType Reference
Name "Switch1"
Ports [2, 1]
- Position [485, 22, 515, 58]
+ Position [485, 92, 515, 128]
NamePlacement "alternate"
SourceBlock "simulink/Signal\nRouting/Manual Switch"
SourceType "Manual Switch"
RTWMemSecDataConstants "Inherit from model"
RTWMemSecDataInternal "Inherit from model"
RTWMemSecDataParameters "Inherit from model"
- sw "1"
+ sw "0"
action "0"
varsize off
}
- Block {
- BlockType Scope
- Name "XY"
- Ports [1]
- Position [870, 29, 900, 61]
- Floating off
- Location [494, 562, 1148, 730]
- Open on
- NumInputPorts "1"
- ZoomMode "xonly"
- List {
- ListType AxesTitles
- axes1 "%<SignalLabel>"
- }
- TimeRange "100"
- YMin "-0.5"
- YMax "3"
- DataFormat "StructureWithTime"
- SampleTime "0"
- }
Block {
BlockType Constant
Name "angle"
- Position [20, 210, 50, 240]
+ Position [20, 180, 50, 210]
}
Block {
BlockType "M-S-Function"
Name "draw probability bitmap"
Ports [1]
- Position [835, 163, 925, 197]
+ Position [565, 243, 655, 277]
+ Orientation "left"
+ ForegroundColor "white"
+ BackgroundColor "black"
FunctionName "sfdrawbitmap"
}
+ Block {
+ BlockType Gain
+ Name "to rad"
+ Position [144, 150, 176, 185]
+ Orientation "up"
+ NamePlacement "alternate"
+ ShowName off
+ Gain "pi/180"
+ ParameterDataTypeMode "Inherit via internal rule"
+ OutDataTypeMode "Inherit via internal rule"
+ SaturateOnIntegerOverflow off
+ }
Block {
BlockType Constant
Name "x"
- Position [20, 65, 50, 95]
+ Position [20, 35, 50, 65]
}
Block {
BlockType Constant
Name "y"
- Position [20, 135, 50, 165]
+ Position [20, 105, 50, 135]
}
Line {
SrcBlock "MCL"
SrcPort 1
Points [40, 0]
Branch {
- DstBlock "Selector1"
- DstPort 1
- }
- Branch {
- Points [0, -60]
- DstBlock "Selector"
+ Points [0, 100]
+ DstBlock "Draw robot"
DstPort 1
}
Branch {
- Points [0, 125]
- DstBlock "Draw robot"
- DstPort 1
+ DstBlock "Graphs"
+ DstPort 2
}
}
- Line {
- SrcBlock "Selector"
- SrcPort 1
- DstBlock "XY"
- DstPort 1
- }
- Line {
- SrcBlock "Selector1"
- SrcPort 1
- DstBlock "Angle"
- DstPort 1
- }
Line {
SrcBlock "MCL"
SrcPort 2
- Points [10, 0; 0, 40]
+ Points [10, 0; 0, 50]
DstBlock "draw probability bitmap"
DstPort 1
}
Line {
SrcBlock "Slider\nGain ANGLE"
SrcPort 1
- Points [30, 0; 0, -65]
- DstBlock "Mux"
- DstPort 3
+ Points [30, 0]
+ DstBlock "to rad"
+ DstPort 1
}
Line {
Labels [0, 0]
DstPort 2
}
Branch {
- Points [0, 110]
+ Points [0, 85]
DstBlock "Draw robot"
DstPort 2
}
DstBlock "Sum"
DstPort 2
}
- Line {
- SrcBlock "Mux"
- SrcPort 1
- Points [10, 0]
- Branch {
- Points [0, 140]
- DstBlock "Draw robot1"
- DstPort 1
- }
- Branch {
- Points [0, -100]
- DstBlock "Subsystem"
- DstPort 1
- }
- Branch {
- Labels [0, 0]
- DstBlock "Rate Transition"
- DstPort 1
- }
- }
Line {
SrcBlock "Switch1"
SrcPort 1
DstBlock "Measurement"
DstPort 1
}
+ Line {
+ SrcBlock "Mux"
+ SrcPort 1
+ Points [10, 0]
+ Branch {
+ DstBlock "Subsystem"
+ DstPort 1
+ }
+ Branch {
+ Points [0, 100]
+ DstBlock "Rate Transition"
+ DstPort 1
+ }
+ Branch {
+ Points [0, -60; 530, 0]
+ Branch {
+ DstBlock "Draw robot1"
+ DstPort 1
+ }
+ Branch {
+ Points [0, 85]
+ DstBlock "Graphs"
+ DstPort 1
+ }
+ }
+ }
+ Line {
+ SrcBlock "to rad"
+ SrcPort 1
+ Points [0, -15]
+ DstBlock "Mux"
+ DstPort 3
+ }
}
}
#define BITMAP_HEIGHT 210
#define PARAM_PART_COUNT_IDX 0
-#define PART_COUNT_PARAM(S) (ssGetSFcnParam(S,PARAM_PART_COUNT_IDX))
+#define PARAM_NOISE_XY_IDX 1
+#define PARAM_NOISE_ANGLE_IDX 2
+#define PARAM_AEVAL_SIGMA_IDX 3
+#define PART_COUNT(S) (*mxGetPr(ssGetSFcnParam(S,PARAM_PART_COUNT_IDX)))
+#define NOISE_XY(S) (*mxGetPr(ssGetSFcnParam(S,PARAM_NOISE_XY_IDX)))
+#define NOISE_ANGLE(S) (*mxGetPr(ssGetSFcnParam(S,PARAM_NOISE_ANGLE_IDX)))
+#define AEVAL_SIGMA(S) (*mxGetPr(ssGetSFcnParam(S,PARAM_AEVAL_SIGMA_IDX)))
#define SAMPLETIME_MOVE 0
#define SAMPLETIME_MEASURE 1
*/
static void mdlCheckParameters(SimStruct *S)
{
- const mxArray *count = PART_COUNT_PARAM(S);
+ const mxArray *count = ssGetSFcnParam(S,PARAM_PART_COUNT_IDX);
if (mxGetM(count) * mxGetN(count) != 1) {
- ssSetErrorStatus(S, "Parameter must be scalar number");
+ ssSetErrorStatus(S, "Parameter 1 must be scalar number");
return;
}
}
#endif
+#define MDL_PROCESS_PARAMETERS
+#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE)
+static void mdlProcessParameters(SimStruct *S)
+{
+ struct mcl_model *mcl = ssGetPWork(S)[0];
+ printf("%s\n", __FUNCTION__);
+ /* Tunable parameters was changed during simulation */
+ if (mcl) {
+ printf("changing mcl\n");
+ mcl->mov_dnoise = NOISE_XY(S);
+ mcl->mov_anoise = NOISE_ANGLE(S);
+ mcl->aeval_sigma = AEVAL_SIGMA(S);
+ }
+}
+#endif
+
+
+
/* Function: mdlInitializeSizes ===============================================
* Abstract:
* The sizes information is used by Simulink to determine the S-function
static void mdlInitializeSizes(SimStruct *S)
{
/* See sfuntmpl.doc for more details on the macros below */
- ssSetNumSFcnParams(S, 1); /* Number of expected parameters */
+ ssSetNumSFcnParams(S, 4); /* Number of expected parameters */
#if defined(MATLAB_MEX_FILE)
if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) {
mcl->width = 3.0; /* in m */
mcl->height = 2.1; /* in m */
/* the noises */
- mcl->mov_dnoise = 0.01;
- mcl->mov_anoise = DEG2RAD(2.0);
+ 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 = DEG2RAD(0.4);
+ mcl->aeval_sigma = AEVAL_SIGMA(S);
mcl->maxavdist = 0.150;
/* bad cycles before reseting */
mcl->maxnoisecycle = 10;
/* amount of particles */
- mcl->count = 3000;
+ mcl->count = PART_COUNT(S);
mcl->parts = (struct mcl_particle *)
malloc(sizeof(struct mcl_particle)*mcl->count);
{
struct mcl_model *mcl = ssGetPWork(S)[0];
free(mcl);
+ ssGetPWork(S)[0] = NULL;
}
/*======================================================*
* See sfuntmpl.doc for the optional S-function methods *
projects / eurobot / public.git / commitdiff