public void UpdateInfo(ScannedRobotEvent evnt, Coordinates robotPostion, double robotHeadings)
{
Bearings.AddNew(evnt.BearingRadians);
Distance.AddNew(evnt.Distance);
Energy.AddNew(evnt.Energy);
Heading.AddNew(evnt.HeadingRadians);
Velocity.AddNew(evnt.Velocity);
Time.AddNew(evnt.Time);
OriginAngle.AddNew(Utils.NormalRelativeAngle(robotHeadings + Bearings.GetCurrent()));
double y = Distance.GetCurrent() * Math.Cos(OriginAngle.GetCurrent());
double x = Distance.GetCurrent() * Math.Sin(OriginAngle.GetCurrent());
Position.AddNew(robotPostion + new Coordinates(x, y));
}