Exemplo n.º 1
0
 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;
 }
Exemplo n.º 2
0
  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;
  }