public SPKF(Control Controller, Data Dataset, Double initQ, Double initR) {
cController = (SPKFControl) Controller;
if (cController.getControlType() == 2) cFController = (FuzzySPKF) Controller;
dDataset = Dataset;
this.setAlpha(0.02);
this.setBeta(2.0);
this.setKappa(0.0);
cController.setL(L);
dQ = initQ;
dR = initR;
}
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;
}