//RConsole.openBluetooth(0);
//System.setOut(new PrintStream(RConsole.openOutputStream()));
// Create the pilot
DifferentialPilot pilot = new DifferentialPilot(
TYRE_DIAMETER, AXLE_TRACK, Motor.B, Motor.C, true);
//Create the filter
KalmanFilter filter = new KalmanFilter(a,b,c,q,r);
// Set the initial state
filter.setState(state, covariance);
// Loop 100 times setting velocity, reading the range and updating the filter
for(int i=0;i<100;i++) {
// Generate a random velocity -20 to +20cm/sec
double velocity = (rand.nextInt(41) - 20);
// Adjust velocity so we keep in range
double position = filter.getMean().get(0, 0);
if (velocity < 0 && position < 20) velocity = -velocity;
if (velocity > 0 && position > 220) velocity = -velocity;
control.set(0, 0, velocity);
System.out.println("Velocity: " + (int) velocity);
// Move the robot
pilot.setMoveSpeed((float) Math.abs(velocity));
if (velocity > 0) pilot.backward();
else pilot.forward();
Thread.sleep(1000);
pilot.stop();
// Take a reading
float range = sonic.getRange();
System.out.println("Range: " + (int) range);
measurement.set(0,0, (double) range);