36 #include <sys/types.h> 39 #include "amcl_odom.h" 48 return atan2(sin(z),cos(z));
51 angle_diff(
double a,
double b)
57 d2 = 2*M_PI - fabs(d1);
60 if(fabs(d1) < fabs(d2))
68 AMCLOdom::AMCLOdom() : AMCLSensor()
74 AMCLOdom::SetModelDiff(
double alpha1,
79 this->model_type = ODOM_MODEL_DIFF;
80 this->alpha1 = alpha1;
81 this->alpha2 = alpha2;
82 this->alpha3 = alpha3;
83 this->alpha4 = alpha4;
87 AMCLOdom::SetModelOmni(
double alpha1,
93 this->model_type = ODOM_MODEL_OMNI;
94 this->alpha1 = alpha1;
95 this->alpha2 = alpha2;
96 this->alpha3 = alpha3;
97 this->alpha4 = alpha4;
98 this->alpha5 = alpha5;
104 bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
107 ndata = (AMCLOdomData*) data;
110 pf_sample_set_t *
set;
112 set = pf->sets + pf->current_set;
113 pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
115 if(this->model_type == ODOM_MODEL_OMNI)
117 double delta_trans, delta_rot, delta_bearing;
118 double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
120 delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
121 ndata->delta.v[1]*ndata->delta.v[1]);
122 delta_rot = ndata->delta.v[2];
125 double trans_hat_stddev = (alpha3 * (delta_trans*delta_trans) +
126 alpha1 * (delta_rot*delta_rot));
127 double rot_hat_stddev = (alpha4 * (delta_rot*delta_rot) +
128 alpha2 * (delta_trans*delta_trans));
129 double strafe_hat_stddev = (alpha1 * (delta_rot*delta_rot) +
130 alpha5 * (delta_trans*delta_trans));
132 for (
int i = 0; i <
set->sample_count; i++)
134 pf_sample_t* sample =
set->samples + i;
136 delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
137 old_pose.v[2]) + sample->pose.v[2];
138 double cs_bearing = cos(delta_bearing);
139 double sn_bearing = sin(delta_bearing);
142 delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
143 delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
144 delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
146 sample->pose.v[0] += (delta_trans_hat * cs_bearing +
147 delta_strafe_hat * sn_bearing);
148 sample->pose.v[1] += (delta_trans_hat * sn_bearing -
149 delta_strafe_hat * cs_bearing);
150 sample->pose.v[2] += delta_rot_hat ;
151 sample->weight = 1.0 /
set->sample_count;
157 double delta_rot1, delta_trans, delta_rot2;
158 double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
159 double delta_rot1_noise, delta_rot2_noise;
163 if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] +
164 ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
167 delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
169 delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
170 ndata->delta.v[1]*ndata->delta.v[1]);
171 delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
176 delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
177 fabs(angle_diff(delta_rot1,M_PI)));
178 delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
179 fabs(angle_diff(delta_rot2,M_PI)));
181 for (
int i = 0; i <
set->sample_count; i++)
183 pf_sample_t* sample =
set->samples + i;
186 delta_rot1_hat = angle_diff(delta_rot1,
187 pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
188 this->alpha2*delta_trans*delta_trans));
189 delta_trans_hat = delta_trans -
190 pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
191 this->alpha4*delta_rot1_noise*delta_rot1_noise +
192 this->alpha4*delta_rot2_noise*delta_rot2_noise);
193 delta_rot2_hat = angle_diff(delta_rot2,
194 pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
195 this->alpha2*delta_trans*delta_trans));
198 sample->pose.v[0] += delta_trans_hat *
199 cos(sample->pose.v[2] + delta_rot1_hat);
200 sample->pose.v[1] += delta_trans_hat *
201 sin(sample->pose.v[2] + delta_rot1_hat);
202 sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
203 sample->weight = 1.0 /
set->sample_count;