48 return atan2(sin(z), cos(z));
51angle_diff(
double a,
double b)
57 d2 = 2 * M_PI - fabs(d1);
60 if (fabs(d1) < fabs(d2))
68AMCLOdom::AMCLOdom() : AMCLSensor()
72 this->model_type = ODOM_MODEL_OMNI;
81AMCLOdom::SetModelDiff(
double alpha1,
double alpha2,
double alpha3,
double alpha4)
83 this->model_type = ODOM_MODEL_DIFF;
84 this->alpha1 = alpha1;
85 this->alpha2 = alpha2;
86 this->alpha3 = alpha3;
87 this->alpha4 = alpha4;
91AMCLOdom::SetModelOmni(
double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5)
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;
104AMCLOdom::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 sqrt(ndata->delta.v[0] * ndata->delta.v[0] + ndata->delta.v[1] * ndata->delta.v[1]);
118 double delta_rot = ndata->delta.v[2];
121 double trans_hat_stddev =
122 (alpha3 * (delta_trans * delta_trans) + alpha1 * (delta_rot * delta_rot));
123 double rot_hat_stddev =
124 (alpha4 * (delta_rot * delta_rot) + alpha2 * (delta_trans * delta_trans));
125 double strafe_hat_stddev =
126 (alpha1 * (delta_rot * delta_rot) + alpha5 * (delta_trans * delta_trans));
128 for (
int i = 0; i < set->sample_count; i++) {
129 pf_sample_t *sample = set->samples + i;
131 double delta_bearing =
132 angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), old_pose.v[2]) + sample->pose.v[2];
133 double cs_bearing = cos(delta_bearing);
134 double sn_bearing = sin(delta_bearing);
137 double delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
138 double delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
139 double delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
141 sample->pose.v[0] += (delta_trans_hat * cs_bearing + delta_strafe_hat * sn_bearing);
142 sample->pose.v[1] += (delta_trans_hat * sn_bearing - delta_strafe_hat * cs_bearing);
143 sample->pose.v[2] += delta_rot_hat;
144 sample->weight = 1.0 / set->sample_count;
149 double delta_rot1, delta_trans, delta_rot2;
150 double delta_rot1_noise, delta_rot2_noise;
154 if (sqrt(ndata->delta.v[1] * ndata->delta.v[1] + ndata->delta.v[0] * ndata->delta.v[0]) < 0.01)
157 delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]), old_pose.v[2]);
159 sqrt(ndata->delta.v[0] * ndata->delta.v[0] + ndata->delta.v[1] * ndata->delta.v[1]);
160 delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
166 std::min(fabs(angle_diff(delta_rot1, 0.0)), fabs(angle_diff(delta_rot1, M_PI)));
168 std::min(fabs(angle_diff(delta_rot2, 0.0)), fabs(angle_diff(delta_rot2, M_PI)));
170 for (
int i = 0; i < set->sample_count; i++) {
171 pf_sample_t *sample = set->samples + i;
174 double delta_rot1_hat =
175 angle_diff(delta_rot1,
176 pf_ran_gaussian(this->alpha1 * delta_rot1_noise * delta_rot1_noise
177 + this->alpha2 * delta_trans * delta_trans));
178 double delta_trans_hat =
180 - pf_ran_gaussian(this->alpha3 * delta_trans * delta_trans
181 + this->alpha4 * delta_rot1_noise * delta_rot1_noise
182 + this->alpha4 * delta_rot2_noise * delta_rot2_noise);
183 double delta_rot2_hat =
184 angle_diff(delta_rot2,
185 pf_ran_gaussian(this->alpha1 * delta_rot2_noise * delta_rot2_noise
186 + this->alpha2 * delta_trans * delta_trans));
189 sample->pose.v[0] += delta_trans_hat * cos(sample->pose.v[2] + delta_rot1_hat);
190 sample->pose.v[1] += delta_trans_hat * sin(sample->pose.v[2] + delta_rot1_hat);
191 sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
192 sample->weight = 1.0 / set->sample_count;