import java.io.PrintStream;
import java.util.Random;
import lejos.robotics.proposal.DifferentialPilot;
import lejos.util.KalmanFilter;
import lejos.util.Matrix;
import lejos.nxt.*;
import lejos.nxt.comm.RConsole;
/**
* Kalman Filter example.
*
* A pilot robot with an ultrasonic sensor facing
* forwards moves backwards and forwards at right angles to a wall.
*
* The state of the system is the distance from the wall.
*
* The control data is the velocity, which is chosen randomly every second.
*
* The measurement data is the ultrasonic sensor range reading to the wall.
*
* The Kalman filter predicts the robot position from its velocity and updates
* the prediction from the range reading.
*
* Put the robot about a meter from the wall and see the estimated mean displayed on
* System.out. (You can modify the program to use RConsole to divert the System.out
* readings to the PC to make it easier to read).
*
* @author Lawrie Griffiths
*
*/
public class KalmanTest {
// Tyre diameter and distance between wheels
private static final float TYRE_DIAMETER = 5.6f;
private static final float AXLE_TRACK = 16.0f;
public static void main(String[] args) throws InterruptedException {
UltrasonicSensor sonic = new UltrasonicSensor(SensorPort.S1);
Random rand = new Random();
Matrix a = new Matrix(new double[][]{{1}}); // Position is only changed by control
Matrix b = new Matrix(new double[][]{{1}}); // Velocity is in cm/sec
Matrix c = new Matrix(new double[][]{{1}}); // Measurement is in cm
Matrix q = new Matrix(new double[][]{{4}}); // Ultrasonic sensor noise factor
Matrix r = new Matrix(new double[][]{{9}}); // Movement noise factor
Matrix state = new Matrix(new double[][]{{100}}); // Start one meter from the wall
Matrix covariance = new Matrix(new double[][]{{100}}); // Big error
Matrix control = new Matrix(1,1);
Matrix measurement = new Matrix(1,1);
//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);
// Update the state
try {
filter.update(control, measurement);
} catch(Exception e) {
System.out.println("Exception: " + e.getClass()+ ":" + e.getMessage());
}
// Print the results
System.out.print("Mean:");
filter.getMean().print(System.out);;
System.out.print("Covariance:");
filter.getCovariance().print(System.out);
}
}
}