initial ekf implementation, without landmark update
results are published, but not plotted
code compiles, but is completely untested