Homepage | Demos | Overview | Downloads | Tutorials | Reference | Credits |
00001 /* Utility routines for handling afsParticle structures in AIBO FastSLAM */ 00002 00003 #ifdef __cplusplus 00004 extern "C" { 00005 #endif 00006 00007 #include "afsParticle.h" 00008 #include <string.h> 00009 00010 void afsParticleInit(afsParticle *p) 00011 { 00012 int i; 00013 00014 p->pose.x = p->pose.y = p->pose.theta = 0; 00015 p->gotweight = 0; 00016 p->weight = 1.0; /* Why 1.0? See afsMeasurementUpdate */ 00017 00018 for(i=0; i<AFS_NUM_LANDMARKS; ++i) { 00019 p->landmarks[i].mean.x = 1980; /* warning value */ 00020 p->landmarks[i].mean.y = 1980; 00021 p->landmarks[i].variance.x = 1980; 00022 p->landmarks[i].variance.y = 1980; 00023 p->landmarks[i].variance.xy = 1980; 00024 00025 p->landmarks[i].state = PRIMING; 00026 p->landmarks[i].priming.empty = 1; 00027 } 00028 } 00029 00030 void afsParticleCopy(afsParticle *old, afsParticle *neu) 00031 { 00032 memcpy(neu, old, sizeof(afsParticle)); 00033 } 00034 00035 #ifdef __cplusplus 00036 } 00037 #endif
Tekkotsu v1.4 |
Generated Sat Jul 19 00:06:29 2003 by Doxygen 1.3.2 |