Skip to content

Commit

Permalink
Merge branch 'vivaan-newAutonImplementation' of https://github.com/CM…
Browse files Browse the repository at this point in the history
…U-Robotics-Club/RoboBuggy into vivaan-newAutonImplementation
  • Loading branch information
sbhotika committed May 25, 2017
2 parents f5e568c + 91fc0aa commit 7113f08
Show file tree
Hide file tree
Showing 27 changed files with 195 additions and 1,928 deletions.
134 changes: 0 additions & 134 deletions offline/controller/waypoints_course.txt

This file was deleted.

733 changes: 0 additions & 733 deletions offline/controller/waypoints_course_v2.txt

This file was deleted.

372 changes: 0 additions & 372 deletions offline/controller/waypoints_tri.txt

This file was deleted.

355 changes: 0 additions & 355 deletions offline/localizer/localizer.java

This file was deleted.

Binary file not shown.
Binary file not shown.
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,8 @@ public final class RobobuggyConfigFile {
public static final boolean LOGGING = true;
public static final String LOG_FILE_LOCATION = "logs";
public static final String LOG_FILE_NAME = "sensors";
public static final double INITIAL_POS_LAT = 40.441670;
public static final double INITIAL_POS_LON = -79.9416362;

//Autonomous controls
public static final int RBSM_COMMAND_PERIOD = 50;
Expand Down
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
package com.roboclub.robobuggy.main;

import com.roboclub.robobuggy.robots.*;
import com.roboclub.robobuggy.simulation.FullSimRunner;
import com.roboclub.robobuggy.robots.AbstractRobot;
import com.roboclub.robobuggy.robots.SimRobot;
import com.roboclub.robobuggy.robots.TransistorAuton;
import com.roboclub.robobuggy.ui.Gui;


Expand Down Expand Up @@ -29,10 +30,7 @@ public static void main(String[] args) {
RobobuggyConfigFile.loadConfigFile(); //TODO make sure that logic Notification is setup before this point

new RobobuggyLogicNotification("Initializing Robot", RobobuggyMessageLevel.NOTE);
// robot = ConfigRobot.getInstance();
// robot = PlayBackRobot.getInstance();
robot = TransistorAuton.getInstance();
// robot = CommTestRobot.getInstance();

new RobobuggyLogicNotification("Initializing GUI", RobobuggyMessageLevel.NOTE);
Gui.getInstance();
Expand Down Expand Up @@ -61,11 +59,6 @@ public static AbstractRobot getCurrentRobot() {
*/
public static void resetSystem() {
robot.shutDown();
// Gui.close();
// Gui.getInstance();
// robot.getInstance();
//TODO make this work for real

}


Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,11 @@
import com.google.gson.JsonObject;
import purejavacomm.CommPortIdentifier;

import java.io.*;
import java.io.File;
import java.io.FileInputStream;
import java.io.FileNotFoundException;
import java.io.InputStreamReader;
import java.io.UnsupportedEncodingException;
import java.util.ArrayList;
import java.util.Enumeration;
import java.util.List;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,13 @@ public DriveControlMessage(Date timestamp, double angle) {
this.timestamp = new Date(timestamp.getTime()).getTime();
}

/**
* Construct a new DriveControlMessage
*
* @param timestamp {@link Date} representing the creation time
* @param angle the commanded angle of the front wheel
* @param currentWaypoint gps measurement representing the current waypoint
*/
public DriveControlMessage(Date timestamp, double angle, GpsMeasurement currentWaypoint) {
this.angle = angle;
this.timestamp = new Date(timestamp.getTime()).getTime();
Expand All @@ -41,6 +48,13 @@ public double getAngleDouble() {
return angle;
}

/**
* Returns the GpsMeasurement representing the current waypoint
* from this DriveControlMessage
*
* @return the GpsMeasurement representing the current waypoint
* from this DriveControlMessage
*/
public GpsMeasurement getWaypoint() {
return currentWaypoint;
}
Expand Down
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
package com.roboclub.robobuggy.messages;

import Jama.Matrix;
import com.roboclub.robobuggy.nodes.localizers.LocalizerUtil;
import com.roboclub.robobuggy.ros.Message;

Expand All @@ -17,11 +16,19 @@ public class GPSPoseMessage extends BaseMessage {

private final double latitude;
private final double longitude;
private final double velocity;
private final double heading;
private final Matrix currentState;

/**
* Constructs a new {@link GPSPoseMessage}
*
* @param timestamp {@link Date} representing the creation time
* @param latitude of the buggy (negative is South)
* @param longitude of the buggy (negative is West)
* @param heading of the buggy (in degrees from North)
*/
public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading) {
this(timestamp, latitude, longitude, heading, null);
this(timestamp, latitude, longitude, heading, 0.0);
}

/**
Expand All @@ -31,13 +38,14 @@ public GPSPoseMessage(Date timestamp, double latitude, double longitude, double
* @param latitude of the buggy (negative is South)
* @param longitude of the buggy (negative is West)
* @param heading of the buggy (in degrees from North)
* @param velocity of the buggy
*/
public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading, Matrix currentState) {
public GPSPoseMessage(Date timestamp, double latitude, double longitude, double heading, double velocity) {
this.latitude = latitude;
this.longitude = longitude;
this.heading = heading;
this.timestamp = new Date(timestamp.getTime()).getTime();
this.currentState = currentState;
this.velocity = velocity;
}

/**
Expand Down Expand Up @@ -67,8 +75,11 @@ public double getHeading() {
return heading;
}

public Matrix getCurrentState() {
return currentState;
/**
* @return the current estimated velocity (m/s)
*/
public double getVelocity() {
return velocity;
}

/**
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -163,21 +163,4 @@ public boolean getNorth() {
public GPSPoseMessage toGpsPoseMessage(double heading) {
return new GPSPoseMessage(gpsTimestamp, latitude, longitude, heading);
}

/**
* evaluates to the distance between two gps points based on an L2 metric
*
* @param a the first gps point
* @param b the second gps point
* @return the distance in meters
*/
public static double getDistance(GpsMeasurement a, GpsMeasurement b) {
double dx = LocalizerUtil.convertLonToMeters(a.getLongitude() - b.getLongitude());
double dy = LocalizerUtil.convertLatToMeters(a.getLatitude() - b.getLatitude());
return Math.sqrt(dx * dx + dy * dy);
}

public int getNumSatellites() {
return numSatellites;
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@ public class RobobuggyKFLocalizer extends PeriodicNode {
// constants we use throughout the file
private static final int X_GLOBAL_ROW = 0;
private static final int Y_GLOBAL_ROW = 1;
private static final int Y_BODY_ROW = 2;
private static final int VELOCITY_ROW = 2;
private static final int HEADING_GLOBAL_ROW = 3;
private static final int HEADING_VEL_ROW = 4;

Expand All @@ -41,22 +41,22 @@ public class RobobuggyKFLocalizer extends PeriodicNode {
// heading - heading, or yaw angle, in the world frame, in rad
// dheading - angular velocity in the world frame, in rad/s
private Matrix x; // current state
private Matrix R; // measurement noise covariance matrix
private Matrix P; // covariance matrix
private Matrix Q_gps; // model noise covariance matrix
private Matrix Q_encoder;

// Q is variance of new measurements
// P is variance of old measurements
// C is observation matrix
// Rk = covariance of measurements
// S-1 models confidence of new measurements
// if Rk is 0, that means kalman gain is high, means you weight measurement more than model
// if Rk is large, kalman gain low, weight model more than measurements
private Matrix rMatrix; // measurement noise covariance matrix
private Matrix pMatrix; // covariance matrix
private Matrix qGPS; // model noise covariance matrix
private Matrix qEncoder;

// output matrices
private Matrix C_gps; // a description of how the GPS impacts x
private Matrix C_encoder; // how the encoder affects x
private Matrix cGPS; // a description of how the GPS impacts x
private Matrix cEncoder; // how the encoder affects x

// Q is the variance of new measurements
// pMatrix is the variance of old measurements
// C is the observation matrix - what variables can we measure, since we can't directly measure everything
// in the state
// rMatrix = covariance of measurements
// if rMatrix is small, that means the "kalman gain" is high, means you weight measurement more than model
// if rMatrix is large, kalman gain low, weight model more than measurements

private long lastTime; // the last time we updated the current position estimate
private UTMTuple lastGPS; // the most recent value of the GPS coordinates, expressed as a UTM measurement
Expand Down Expand Up @@ -95,32 +95,32 @@ public RobobuggyKFLocalizer(int period, String name, LocTuple initialPosition) {
double[] rArray = {4, 4, 0.25, 0.01, 0.01};
double[] pArray = {25, 25, 0.25, 2.46, 2.46};

R = arrayToMatrix(rArray);
P = arrayToMatrix(pArray);
rMatrix = arrayToMatrix(rArray);
pMatrix = arrayToMatrix(pArray);

double[][] qGPS2D = {
{1, 0, 0},
{0, 1, 0},
{0, 0, 0.01},
};
Q_gps = new Matrix(qGPS2D);
qGPS = new Matrix(qGPS2D);

double[][] qEncoder2D = {
{0.25},
};
Q_encoder = new Matrix(qEncoder2D);
qEncoder = new Matrix(qEncoder2D);

double[][] cGPS2D = {
{1, 0, 0, 0, 0},
{0, 1, 0, 0, 0},
{0, 0, 0, 1, 0},
};
C_gps = new Matrix(cGPS2D);
cGPS = new Matrix(cGPS2D);

double[][] cEncoder2D = {
{0, 0, 1, 0, 0},
};
C_encoder = new Matrix(cEncoder2D);
cEncoder = new Matrix(cEncoder2D);

// add all our subscribers for our current state update stream
// Every time we get a new sensor update, trigger the new kalman update
Expand Down Expand Up @@ -153,7 +153,7 @@ private void setupEncoderSubscriber() {
};
Matrix z = new Matrix(z2D);

kalmanFilter(C_encoder, Q_encoder, z);
kalmanFilter(cEncoder, qEncoder, z);
}));
}

Expand Down Expand Up @@ -188,7 +188,7 @@ private void setupGPSSubscriber() {

Matrix z = new Matrix(z2D);

kalmanFilter(C_gps, Q_gps, z);
kalmanFilter(cGPS, qGPS, z);
}));
}

Expand All @@ -201,14 +201,14 @@ private void setupSteeringSubscriber() {

@Override
protected void update() {
Matrix x_predict = propagate();
Matrix xPredict = propagate();

UTMTuple utm = new UTMTuple(UTMZONE, 'T', x_predict.get(X_GLOBAL_ROW, 0),
x_predict.get(Y_GLOBAL_ROW, 0));
UTMTuple utm = new UTMTuple(UTMZONE, 'T', xPredict.get(X_GLOBAL_ROW, 0),
xPredict.get(Y_GLOBAL_ROW, 0));
LocTuple latLon = LocalizerUtil.utm2Deg(utm);
if (gpsMessagesReceived) {
posePub.publish(new GPSPoseMessage(new Date(), latLon.getLatitude(),
latLon.getLongitude(), x_predict.get(HEADING_GLOBAL_ROW, 0), x));
latLon.getLongitude(), xPredict.get(HEADING_GLOBAL_ROW, 0), xPredict.get(VELOCITY_ROW, 0)));
}
}

Expand Down Expand Up @@ -238,13 +238,13 @@ private Matrix getMotionModel(double dt) {
| |
------------------------------------------------------------------------------------|
*/
private void kalmanFilter(Matrix C, Matrix Q, Matrix z) {
private void kalmanFilter(Matrix cMatrix, Matrix qMatrix, Matrix z) {
// update time
Date now = new Date();
double dt = (now.getTime() - lastTime) / 1000.0;
lastTime = now.getTime();

Matrix A = getMotionModel(dt);
Matrix aMatrix = getMotionModel(dt);

/*
the predict step is responsible for determining the estimate of the next state
Expand All @@ -254,14 +254,14 @@ prediction state (xhat[k]) and our prediction noise (phat[k])
Predict:
x_pre = A * x
P_pre = A * P * A' + R
P_pre = A * pMatrix * A' + rMatrix
*/
Matrix x_pre = A.times(x);
Matrix P_pre = A.times(P).times(A.transpose());
P_pre = P_pre.plus(R);
Matrix xPre = aMatrix.times(x);
Matrix pPre = aMatrix.times(pMatrix).times(aMatrix.transpose());
pPre = pPre.plus(rMatrix);

x_pre.set(HEADING_GLOBAL_ROW, 0, clampAngle(x_pre.get(HEADING_GLOBAL_ROW, 0)));
x_pre.set(HEADING_VEL_ROW, 0, clampAngle(x_pre.get(HEADING_VEL_ROW, 0)));
xPre.set(HEADING_GLOBAL_ROW, 0, clampAngle(xPre.get(HEADING_GLOBAL_ROW, 0)));
xPre.set(HEADING_VEL_ROW, 0, clampAngle(xPre.get(HEADING_VEL_ROW, 0)));

/*
the update step is responsible for updating the current state, and resolving
Expand All @@ -278,33 +278,18 @@ This produces your next state (x[k]) and your next noise estimation (p[k]),
r = z - (C * x_pre)
K = P_pre * C' * inv((C * P_pre * C') + Q) // gain
x = x_pre + (K * r)
P = (I - (K * C)) * P_pre
pMatrix = (I - (K * C)) * P_pre
*/
Matrix residual = z.minus(C.times(x_pre));
Matrix K = C.times(P_pre);
K = K.times(C.transpose());
K = K.plus(Q);
K = P_pre.times(C.transpose()).times(K.inverse());

double prev_heading = x.get(HEADING_GLOBAL_ROW, 0);
x = x_pre.plus(K.times(residual));
double new_heading = x.get(HEADING_GLOBAL_ROW, 0);

// if (Math.abs(prev_heading - new_heading) > Math.toRadians(45)) {
// if (x.get(2, 0) > 0.5) {
// if (new_heading > prev_heading) {
// x.set(HEADING_GLOBAL_ROW, 0, prev_heading - Math.toRadians(10));
// } else {
// x.set(HEADING_GLOBAL_ROW, 0, prev_heading + Math.toRadians(10));
// }
// }
// else {
// x.set(HEADING_GLOBAL_ROW, 0, prev_heading);
// }
// }

P = Matrix.identity(5, 5).minus(K.times(C));
P = P.times(P_pre);
Matrix residual = z.minus(cMatrix.times(xPre));
Matrix kMatrix = cMatrix.times(pPre);
kMatrix = kMatrix.times(cMatrix.transpose());
kMatrix = kMatrix.plus(qMatrix);
kMatrix = pPre.times(cMatrix.transpose()).times(kMatrix.inverse());

x = xPre.plus(kMatrix.times(residual));

pMatrix = Matrix.identity(5, 5).minus(kMatrix.times(cMatrix));
pMatrix = pMatrix.times(pPre);

x.set(HEADING_GLOBAL_ROW, 0, clampAngle(x.get(HEADING_GLOBAL_ROW, 0)));
x.set(HEADING_VEL_ROW, 0, clampAngle(x.get(HEADING_VEL_ROW, 0)));
Expand All @@ -317,8 +302,8 @@ private Matrix propagate() {
double dt = (now.getTime() - lastTime) / 1000.0;

// x_pre = A * x
Matrix A = getMotionModel(dt);
return A.times(x);
Matrix aMatrix = getMotionModel(dt);
return aMatrix.times(x);
}


Expand Down
Loading

0 comments on commit 7113f08

Please sign in to comment.