private void RunSPKF() {
// SPKF Steps:
// 1) Generate Test Points
// 2) Propagate Test Points
// 3) Compute Predicted Mean and Covariance
// 4) Compute Measurements
// 5) Compute Innovations and Cross Covariance
// 6) Compute corrections and update
// Line up initial variables from the controller!
Double dAlpha = dGreek.get(0);
Double dBeta = dGreek.get(1);
cController.setAlpha(dAlpha);
cController.setBeta(dBeta);
cController.setKappa(dGreek.get(2));
Double dGamma = cController.getGamma();
Double dLambda = cController.getLambda();
// // DEBUG - Print the Greeks
// System.out.println("Greeks!");
// System.out.println("Alpha - " + dAlpha);
// System.out.println("Beta - " + dBeta);
// System.out.println("Kappa - " + dGreek.get(2));
// System.out.println("Lambda - " + dLambda);
// System.out.println("Gamma - " + dGamma);
// Let's get started:
// Step 1: Generate Test Points
Vector<Matrix> Chi = new Vector<Matrix>();
Vector<Matrix> UpChi = new Vector<Matrix>();
Vector<Matrix> UpY = new Vector<Matrix>();
Matrix UpPx = new Matrix(3, 3, 0.0);
Matrix UpPy = new Matrix(3, 3, 0.0);
Matrix UpPxy = new Matrix(3, 3, 0.0);
Matrix K;
Vector<Double> wc = new Vector<Double>();
Vector<Double> wm = new Vector<Double>();
Chi.add(X); // Add Chi_0 - the current state estimate (X, Y, Z)
// Big P Matrix is LxL diagonal
Matrix SqrtP = SqrtSPKF(P);
SqrtP = SqrtP.times(dGamma);
// Set up Sigma Points
for (int i = 0; i <= 8; i++) {
Matrix tempVec = SqrtP.getMatrix(0, 8, i, i);
Matrix tempX = X;
Matrix tempPlus = tempX.plus(tempVec);
// System.out.println("TempPlus");
// tempPlus.print(3, 2);
Matrix tempMinu = tempX.minus(tempVec);
// System.out.println("TempMinus");
// tempMinu.print(3, 2);
// tempX = X.copy();
// tempX.setMatrix(i, i, 0, 2, tempPlus);
Chi.add(tempPlus);
// tempX = X.copy();
// tempX.setMatrix(i, i, 0, 2, tempMinu);
Chi.add(tempMinu);
}
// DEBUG Print the lines inside the Chi Matrix (2L x L)
// for (int i = 0; i<=(2*L); i++){
// System.out.println("Chi Matrix Set: "+i);
// Chi.get(i).print(5, 2);
// }
// Generate weights
Double WeightZero = (dLambda / (L + dLambda));
Double OtherWeight = (1 / (2 * (L + dLambda)));
Double TotalWeight = WeightZero;
wm.add(WeightZero);
wc.add(WeightZero + (1 - (dAlpha * dAlpha) + dBeta));
for (int i = 1; i <= (2 * L); i++) {
TotalWeight = TotalWeight + OtherWeight;
wm.add(OtherWeight);
wc.add(OtherWeight);
}
// Weights MUST BE 1 in total
for (int i = 0; i <= (2 * L); i++) {
wm.set(i, wm.get(i) / TotalWeight);
wc.set(i, wc.get(i) / TotalWeight);
}
// //DEBUG Print the weights
// System.out.println("Total Weight:");
// System.out.println(TotalWeight);
// for (int i = 0; i<=(2*L); i++){
// System.out.println("Weight M for "+i+" Entry");
// System.out.println(wm.get(i));
// System.out.println("Weight C for "+i+" Entry");
// System.out.println(wc.get(i));
// }
// Step 2: Propagate Test Points
// This will also handle computing the mean
Double ux = dControl.elementAt(0);
Double uy = dControl.elementAt(1);
Double uz = dControl.elementAt(2);
Matrix XhatMean = new Matrix(3, 1, 0.0);
for (int i = 0; i < Chi.size(); i++) {
Matrix ChiOne = Chi.get(i);
Matrix Chixminus = new Matrix(3, 1, 0.0);
Double Xhat = ChiOne.get(0, 0);
Double Yhat = ChiOne.get(1, 0);
Double Zhat = ChiOne.get(2, 0);
Double Xerr = ChiOne.get(3, 0);
Double Yerr = ChiOne.get(4, 0);
Double Zerr = ChiOne.get(5, 0);
Xhat = Xhat + ux + Xerr;
Yhat = Yhat + uy + Yerr;
Zhat = Zhat + uz + Zerr;
Chixminus.set(0, 0, Xhat);
Chixminus.set(1, 0, Yhat);
Chixminus.set(2, 0, Zhat);
// System.out.println("ChixMinus:");
// Chixminus.print(3, 2);
UpChi.add(Chixminus);
XhatMean = XhatMean.plus(Chixminus.times(wm.get(i)));
}
// Mean is right!
// System.out.println("XhatMean: ");
// XhatMean.print(3, 2);
// Step 3: Compute Predicted Mean and Covariance
// Welp, we already solved the mean - let's do the covariance now
for (int i = 0; i <= (2 * L); i++) {
Matrix tempP = UpChi.get(i).minus(XhatMean);
Matrix tempPw = tempP.times(wc.get(i));
tempP = tempPw.times(tempP.transpose());
UpPx = UpPx.plus(tempP);
}
// New Steps!
// Step 4: Compute Measurements! (and Y mean!)
Matrix YhatMean = new Matrix(3, 1, 0.0);
for (int i = 0; i <= (2 * L); i++) {
Matrix ChiOne = Chi.get(i);
Matrix Chiyminus = new Matrix(3, 1, 0.0);
Double Xhat = UpChi.get(i).get(0, 0);
Double Yhat = UpChi.get(i).get(1, 0);
Double Zhat = UpChi.get(i).get(2, 0);
Double Xerr = ChiOne.get(6, 0);
Double Yerr = ChiOne.get(7, 0);
Double Zerr = ChiOne.get(8, 0);
Xhat = Xhat + Xerr;
Yhat = Yhat + Yerr;
Zhat = Zhat + Zerr;
Chiyminus.set(0, 0, Xhat);
Chiyminus.set(1, 0, Yhat);
Chiyminus.set(2, 0, Zhat);
UpY.add(Chiyminus);
YhatMean = YhatMean.plus(Chiyminus.times(wm.get(i)));
}
// // Welp, we already solved the mean - let's do the covariances
// now
// System.out.println("XHatMean and YHatMean = ");
// XhatMean.print(3, 2);
// YhatMean.print(3, 2);
for (int i = 0; i <= (2 * L); i++) {
Matrix tempPx = UpChi.get(i).minus(XhatMean);
Matrix tempPy = UpY.get(i).minus(YhatMean);
// System.out.println("ChiX - XhatMean and ChiY-YhatMean");
// tempPx.print(3, 2);
// tempPy.print(3, 2);
Matrix tempPxw = tempPx.times(wc.get(i));
Matrix tempPyw = tempPy.times(wc.get(i));
tempPx = tempPxw.times(tempPy.transpose());
tempPy = tempPyw.times(tempPy.transpose());
UpPy = UpPy.plus(tempPy);
UpPxy = UpPxy.plus(tempPx);
}
// Step 6: Compute Corrections and Update
// Compute Kalman Gain!
// System.out.println("Updated Px");
// UpPx.print(5, 2);
// System.out.println("Updated Py");
// UpPy.print(5, 2);
// System.out.println("Updated Pxy");
// UpPxy.print(5, 2);
K = UpPxy.times(UpPy.inverse());
// System.out.println("Kalman");
// K.print(5, 2);
Matrix Mea = new Matrix(3, 1, 0.0);
Mea.set(0, 0, dMeasure.get(0));
Mea.set(1, 0, dMeasure.get(1));
Mea.set(2, 0, dMeasure.get(2));
Matrix Out = K.times(Mea.minus(YhatMean));
Out = Out.plus(XhatMean);
// System.out.println("Out:");
// Out.print(3, 2);
Matrix Px = UpPx.minus(K.times(UpPy.times(K.transpose())));
// Update Stuff!
// Push the P to the controller
Matrix OutP = P.copy();
OutP.setMatrix(0, 2, 0, 2, Px);
X.setMatrix(0, 2, 0, 0, Out);
Residual = XhatMean.minus(Out);
cController.inputState(OutP, Residual);
// cController.setL(L);
cController.startProcess();
while (!cController.finishedProcess()) {
try {
Thread.sleep(10);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
// System.out.println("Post Greeks: " + cController.getAlpha() + " ,
// "+ cController.getBeta());
dGreek.set(0, cController.getAlpha());
dGreek.set(1, cController.getBeta());
dGreek.set(2, cController.getKappa());
P = cController.getP();
// System.out.println("P is post Process:");
// P.print(3, 2);
StepDone = true;
}
public void startProcess() {
// Generate new Variables
GPSEntry gGPS;
IMUEntry iIMU;
OdoEntry oOdo;
CompassEntry cComp;
Double dTS;
Vector<Double> vIMU = new Vector<Double>();
Vector<Double> vComp = new Vector<Double>();
Vector<Double> DeltaH = new Vector<Double>();
Vector<Double> tempOdo = new Vector<Double>();
Vector<Double> vGPS = new Vector<Double>();
Vector<Double> vGreekHeading = new Vector<Double>();
Vector<Double> vGreekHeading1 = new Vector<Double>();
Matrix Deviation = new Matrix(2, 1, 0.0);
// Stores Alpha,
// Beta and
// Kappa for
// each type of
// controller
Vector<Double> vGreekDrive = new Vector<Double>(); // Stores Alpha,
// Beta and Kappa
// for each type of
// controller
Vector<Double> vGreekPose = new Vector<Double>(); // Stores Alpha,
// Beta and Kappa
// for each type of
// controller
// Start the process!
ProcessOn = true;
TimeOn = new Date();
// Get the length of the dataset
int iLength = dDataset.getLength();
// Build the First States
vGreekHeading.add(defA);
vGreekHeading.add(defB);
vGreekHeading.add(defK);
vGreekDrive = vGreekPose = vGreekHeading1 = vGreekHeading;
Matrix HeadingX = new Matrix(9, 1, 0.0);
Matrix HeadingP = new Matrix(9, 9, 0.0);
Matrix Heading1X = new Matrix(9, 1, 0.0);
Matrix Heading1P = new Matrix(9, 9, 0.0);
Matrix DriveX = new Matrix(9, 1, 0.0);
Matrix DriveP = new Matrix(9, 9, 0.0);
Matrix PoseX = new Matrix(9, 1, 0.0);
Matrix PoseP = new Matrix(9, 9, 0.0);
// Reset the P values to norm
HeadingP.set(3, 3, dQ);
HeadingP.set(4, 4, dQ);
HeadingP.set(5, 5, dQ);
HeadingP.set(6, 6, dR);
HeadingP.set(7, 7, dR);
HeadingP.set(8, 8, dR);
Heading1P.set(3, 3, dQ);
Heading1P.set(4, 4, dQ);
Heading1P.set(5, 5, dQ);
Heading1P.set(6, 6, dR);
Heading1P.set(7, 7, dR);
Heading1P.set(8, 8, dR);
DriveP.set(3, 3, dQ);
DriveP.set(4, 4, dQ);
DriveP.set(5, 5, dQ);
DriveP.set(6, 6, dR);
DriveP.set(7, 7, dR);
DriveP.set(8, 8, dR);
PoseP.set(3, 3, dQ);
PoseP.set(4, 4, dQ);
PoseP.set(5, 5, dQ);
PoseP.set(6, 6, 2.5);
PoseP.set(7, 7, 2.5);
PoseP.set(8, 8, 2.5);
gGPS = dDataset.GetGPSEntry(0);
Heading1X.set(2, 0, dDataset.GetOdoEntry(0).getOdoTheta());
HeadingX.set(2, 0, dDataset.GetCompassEntry(0).GetRad());
DriveX.set(0, 0, dDataset.GetOdoEntry(0).getOdoX());
DriveX.set(1, 0, dDataset.GetOdoEntry(0).getOdoY());
PoseX.set(0, 0, gGPS.getEastX());
PoseX.set(1, 0, gGPS.getNorthY());
Double OldTheta = 0.0;
Double OldGPSX = gGPS.getEastX();
Double OldGPSY = gGPS.getNorthY();
Double OldOdoX = 0.0;
Double OldOdoY = 0.0;
Double OldComp = dDataset.GetCompassEntry(0).GetRad();
// Start the roll!
for (int i = 0; i < iLength; i++) {
// Get the data @ i
gGPS = dDataset.GetGPSEntry(i);
iIMU = dDataset.GetIMUEntry(i);
oOdo = dDataset.GetOdoEntry(i);
dTS = gGPS.getGPSTimestamp();
cComp = dDataset.GetCompassEntry(i);
// vRIMU = dDataset.GetIMURotVelocity(i);
// Cascading SPKFs!
// Sigma Point Kalman Filtering Section
// Step 1: SPKF IMU Rotation (Acceleration in Robot) with Odo
// Heading (Delta in World) to get ImpDeltaH
// Step 2: SPKF ImpDelta (Delta Heading in World) with Heading in
// World to get TrueHeading
// Step 3: SPKF IMU Acceleration combined TrueHeading with Odo
// (Delta Location in World) to get ImpDeltaL
// Step 4: SPKF ImpDeltaL (Delta Location in World) with GPS
// Location (Location in World) to get TrueLoc
// Step 1:
tempOdo.clear();
tempOdo.add(0.0);
tempOdo.add(0.0);
tempOdo.add(oOdo.getOdoTheta() - OldTheta);
OldTheta = oOdo.getOdoTheta();
vIMU.clear();
vIMU.add(0.0);
vIMU.add(0.0);
vIMU.add(iIMU.getIMURotAccel().get(2) * dTS);
SPKFStep HeadingOne = new SPKFStep(Heading1P, Heading1X, vIMU, tempOdo, vGreekHeading1, dTS);
while (!HeadingOne.StepFinished()) {
try {
Thread.sleep(10);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
Heading1P = HeadingOne.getP();
Heading1X = HeadingOne.getX();
vGreekHeading1 = HeadingOne.getGreek();
// OutputH1.add(new CompassEntry(Heading1X.get(2, 0), dTS, true));
// ResidH1.add(HeadingOne.getResid());
// Step 2:
vComp.clear();
vComp.add(0.0);
vComp.add(0.0);
vComp.add(cComp.GetRad());
tempOdo.clear();
tempOdo.add(Heading1X.get(0, 0));
tempOdo.add(Heading1X.get(1, 0));
tempOdo.add(Heading1X.get(2, 0));
// System.out.println("Heading Before at Step " + i + ":");
// HeadingP.print(3, 2);
// HeadingX.print(3, 2);
SPKFStep Heading = new SPKFStep(HeadingP, HeadingX, tempOdo, vComp, vGreekHeading, dTS);
while (!Heading.StepFinished()) {
try {
Thread.sleep(10);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
HeadingP = Heading.getP();
HeadingX = Heading.getX();
vGreekHeading = Heading.getGreek();
// OutputH.add(new CompassEntry(HeadingX.get(2, 0), dTS, true));
// ResidH.add(Heading.getResid());
// System.out.println("Heading After at Step " + i + ":");
// HeadingP.print(3, 2);
// HeadingX.print(3, 2);
DeltaH.clear();
DeltaH.add(HeadingX.get(0, 0));
DeltaH.add(HeadingX.get(1, 0));
DeltaH.add(HeadingX.get(2, 0));
// Step 3:
vIMU = dDataset.GetIMUVelocity(i, DeltaH);
for (int j = 0; j < vIMU.size(); j++) {
vIMU.set(j, (vIMU.get(j) * dTS));
}
// This yields Velocity -^
// Step 3:
tempOdo.clear();
tempOdo.add(oOdo.getOdoX() - OldOdoX);
tempOdo.add(oOdo.getOdoY() - OldOdoY);
tempOdo.add(0.0);
OldOdoX = oOdo.getOdoX();
OldOdoY = oOdo.getOdoY();
// //
// System.out.println("Drive Before at Step " + i + ":");
// DriveP.print(3, 2);
// DriveX.print(3, 2);
SPKFStep Drive = new SPKFStep(DriveP, DriveX, vIMU, tempOdo, vGreekDrive, dTS);
while (!Drive.StepFinished()) {
try {
Thread.sleep(10);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
DriveP = Drive.getP();
DriveX = Drive.getX();
vGreekDrive = Drive.getGreek();
// OutputD.add(new OdoEntry(DriveX.get(0, 0), DriveX.get(1, 0),
// DeltaH.get(2), dTS));
// ResidD.add(Drive.getResid());
// System.out.println("Drive After at Step " + i + ":");
// DriveP.print(3, 2);
// DriveX.print(3, 2);
// Step 4:
tempOdo.clear();
tempOdo.add(DriveX.get(0, 0));
tempOdo.add(DriveX.get(1, 0));
tempOdo.add(DriveX.get(2, 0));
vGPS.clear();
vGPS.add(gGPS.getEastX()); // -OldGPSX);
vGPS.add(gGPS.getNorthY()); // -OldGPSY);
vGPS.add(0.0);
if (cController.getControlType() == 2) {
for (int j = 0; j < vIMU.size(); j++) { // This yields single
// step motion
vIMU.set(j, (vIMU.get(j) * dTS));
}
Deviation.set(0, 0, gGPS.getEastX() - ((vIMU.get(0) * dTS) + OldGPSX));
Deviation.set(1, 0, gGPS.getNorthY() - ((vIMU.get(1) * dTS) + OldGPSY));
cFController = (FuzzySPKF) cController;
cFController.isGPS(Deviation);
cController = cFController;
}
OldGPSX = gGPS.getEastX();
OldGPSY = gGPS.getNorthY();
// System.out.println("Pose Before at Step " + i + ":");
// PoseP.print(3, 2);
// PoseX.print(3, 2);
SPKFStep Pose = new SPKFStep(PoseP, PoseX, tempOdo, vGPS, vGreekPose, dTS);
while (!Pose.StepFinished()) {
try {
Thread.sleep(10);
} catch (InterruptedException e) {
e.printStackTrace();
}
}
PoseP = Pose.getP();
PoseX = Pose.getX();
vGreekPose = Pose.getGreek();
// System.out.println("GPS :" + gGPS.getEastX() + ", " +
// gGPS.getNorthY());
// System.out.println("Pose After at Step " + i + ":");
// PoseP.print(3, 2);
// PoseX.print(3, 2);
GPSEntry GPSOut = new GPSEntry(PoseX.get(0, 0), PoseX.get(1, 0), dTS);
// System.out.println(GPSOut.toString());
Output.add(GPSOut);
Resid.add(Pose.getResid());
// Ticker
if (i % 100 == 0) System.out.print(".");
}
// Process done!
TimeOff = new Date();
TimeElapsed = TimeOff.getTime() - TimeOn.getTime();
ProcessOn = false;
ProcessDone = true;
}
