2 * This file is part of MCL library.
4 * MCL library is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published
6 * by the Free Software Foundation, either version 3 of the License,
7 * or (at your option) any later version.
9 * MCL library is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 * General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with MCL library. If not, see <http://www.gnu.org/licenses/>.
17 * Copyright (C) 2007 - 2009 Czech Technical University in Prague
19 * Authors: Michal Sojka <sojkam1@fel.cvut.cz>
28 #include <mcl_robot.h>
32 #define mcl_to_laser(mcl) (struct mcl_laser*)((mcl)->priv)
34 #define MCL_LASER_MAX_MEASURE 10
36 struct mcl_laser_state {
41 int8_t beacon; /* from which beacon we expect the next reflection */
44 struct mcl_laser_measurement {
46 double val[MCL_LASER_MAX_MEASURE];
49 typedef double mcl_thetas[BEACON_CNT];
53 struct mcl_laser_state *parts; /* pointer to particles */
54 struct mcl_robot_pos estimated; /* current best estimated position */
55 double pred_dnoise; /* prediction distance noises */
56 double pred_anoise; /* prediction angle noises */
57 #if 1 /* Currently not used */
58 double beacon_miss; /* beacon miss probability */
59 double false_beacon; /* false beacon probability */
61 /* used to evaluate the particles */
62 double aeval_sigma; /* sigma used for angles evaluating */
63 /* size of the playground (in meters) */
66 unsigned char beacon_color; /* red/blue beacons */
68 /* Temporary arrays for resampling */
70 struct mcl_laser_state *resampled;
76 struct mcl_model *mcl_laser_init(struct mcl_laser *l, unsigned count);
77 double mcl_beacon_angle(const struct mcl_laser_state *part,
79 void mcl_pos2ang(const struct mcl_laser_state *part,
81 unsigned char cnt, unsigned char color);