void MonteCarlo_UpdatePosition(Object state)
{
ts_position_t position = new ts_position_t();
position.theta = (float)(actualPosition.angle * 180 / Math.PI);
position.x = 1000 * actualPosition.x + SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2);
position.y = 1000 * actualPosition.y + SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2);
ts_scan_t scan = this.doScan();
int quality;
ts_position_t newPos = tinySLAM.MonteCarlo_UpdatePosition(scan, position, 1, 1, 10000, 50, out quality);
Position p = new Position();
p.x = (newPos.x - SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2)) * 1e-3;
p.y = (newPos.y - SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2)) * 1e-3;
p.angle = newPos.theta * Math.PI / 180;
Position diff = p - actualPosition;
//Only the new position will be sent if is near of the actual position
if (diff.x < 3e-2 && diff.y < 3e-2 && diff.angle < 5 * Math.PI / 180)
{
coder.Send(Message.UpdatePosition, p);
}
}
public void Scan(object state)
{
UpdaterInt d = new UpdaterInt(updateIterationsLabel);
this.Dispatcher.Invoke(d, tinySLAM.NumUpdates());
ts_scan_t scan = this.doScan();
ts_position_t position = new ts_position_t();
position.theta = (float)(actualPosition.angle * 180 / Math.PI);
position.x = 1000 * actualPosition.x + SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2);
position.y = 1000 * actualPosition.y + SLAMAlgorithm.TS_MAP_SIZE / (SLAMAlgorithm.TS_MAP_SCALE * 2);
tinySLAM.NewScan(scan, position);
}