Fix sensor vision check

Fixed vision check methods in trilaterator.cs

Ported KalmanFilter.cs to Octave. There is no longer any dependancy on
Matlab.

Tweaked DisplayServer image output a little so estimates do not hide
trilateration results when overlapping perfectly.