| 41 | } |
| 42 | |
| 43 | void MotionModel::updateParticle( |
| 44 | Particle & particle, double dt, |
| 45 | geometry_msgs::msg::Twist::SharedPtr cmd_msg) |
| 46 | { |
| 47 | particle.x += cos(particle.yaw) * particle.x_vel * dt + sigmas_.x * gaussian_noise_.Sample() * |
| 48 | sqrt( |
| 49 | dt); |
| 50 | particle.y += -sin(particle.yaw) * particle.x_vel * dt + sigmas_.y * gaussian_noise_.Sample() * |
| 51 | sqrt( |
| 52 | dt); |
| 53 | particle.yaw += particle.yaw_vel * dt + sigmas_.yaw * gaussian_noise_.Sample() * sqrt(dt); |
| 54 | |
| 55 | particle.x_vel = cmd_msg->linear.x + sigmas_.x_vel * gaussian_noise_.Sample() * sqrt(dt); |
| 56 | particle.yaw_vel = -cmd_msg->angular.z + sigmas_.yaw_vel * gaussian_noise_.Sample() * sqrt(dt); |
| 57 | |
| 58 | particle.yaw = angles::normalize_angle(particle.yaw); |
| 59 | } |
| 60 | |
| 61 | void MotionModel::updateParticles( |
| 62 | std::vector<Particle> & particles, |