Over the past week, I've successfully transforming the range-bearing measurements to direct observation with correct statistics (without approximation) Now the algorithm is fully functional.
Here is the demo setting:
- maximum range radius: 25 cm
- range uncertainty: 0.5 cm^2
- bearing uncertainty: 0.01 rad^2
- time update frequency: 10 Hz
- observation frequency: 5 Hz