Plotting the objects on the map
My first mistake was to try working in degrees. It made sense when I started as I could see what was going on but as the mathematics get more complicated I need to be working in radians. Now is the time to ensure that all the internal variables are in radians and they only become degrees for display purposes.
To plot the location of an object on the map the equation above applies, as C code it is;
ObjectLocation = robotLocation + (distanceToObject * cos(BearingToObjectInRadians));
As you can see there is no reason for the internal workings of the code to need degrees and it only results in having to convert back and forth further compounding the errors we already have in the system.
To convert degrees to radians you just need to know that 360 degrees is 2*M_PI or:
radians = degrees * (M_PI / 180);
So now you have the location of the object relative to the robot you need to record that on the map. Eric uses a 10,000 by 10,000 matrix held in the internal memory and by defining this as a one centimetre grid we get a 100 meeter map to work with.