Adjusted Kalman filters

TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl

@@ -8,18 +8,17 @@ component Mastercomponent {
         out  Q(-1:1)^{3}           commandsOut;
 
    instance Dpnet dpnet;
-   instance Drivercontroller driverController;
+   instance DriverController driverController;
    instance Unnormalizer unnorm;
-   //instance KFMastercomponent kfm;
+   instance KFMastercomponent kfm;
    instance SteeringBuffer steeringBuffer;
 
    connect imageIn -> dpnet.data;
    connect dpnet.predictions -> unnorm.normalizedPredictions;
 
-   //connect un.affordance -> kfm.predictions;
-   //connect kfm.predictionsSmoothed -> driverController.affordanceIn;
+   connect unnorm.affordance -> kfm.affordanceIn;
+   connect kfm.affordanceSmoothed -> driverController.affordanceIn;
 
-   connect unnorm.affordance -> driverController.affordanceIn;
    connect steeringBuffer.outputBuffer -> driverController.steeringRecordIn;
    connect speedIn -> driverController.speedIn;
 

TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl → TorcsEMAMGenerator/src/main/models/dp/subcomponents/DriverController.emadl

 package dp.subcomponents;
 
-component Drivercontroller {
+component DriverController {
     ports
         in Q^{5} steeringRecordIn,
 	    in Affordance affordanceIn,
@@ -17,7 +17,8 @@ component Drivercontroller {
         Q desiredSpeed;
 	    Q slowDown = 100; // Acceleration
 
-	    if (affordanceIn.distMM < 15)
+        // Car following speed model: if a car is less than 20m ahead
+	    if (affordanceIn.distMM < 20)
             Q vMax = 20;
             Q vC = 2.772;
             Q vD = -0.693;
@@ -46,7 +47,6 @@ component Drivercontroller {
             coeSteer = 0.3;
         end
 
-        Q steerCmd = 0;
         // steering control, "shared->steerCmd" [-1,1] is the value sent back to TORCS
 	    steerCmd = (affordanceIn.angle - centerLine/roadWidth) / 0.541052 / coeSteer;
 
@@ -55,7 +55,7 @@ component Drivercontroller {
         end
         commandsOut(2) = steerCmd;
 
-        desiredSpeed = 20;
+        desiredSpeed = 20; // 20m/s = 72 kmh
 	    if (affordanceIn.fast != 1)
 	        desiredSpeed = 20 - abs(steeringRecordIn(0) + steeringRecordIn(1) + steeringRecordIn(2) + steeringRecordIn(3) + steeringRecordIn(4)) * 4.5;
 	    end

TorcsEMAMGenerator/src/main/models/dp/subcomponents/KFMastercomponent.emadl

 package dp.subcomponents;
 
 component KFMastercomponent {
-    ports in Affordance predictions,
-          out Affordance predictionsSmoothed;
+    ports in Affordance affordanceIn,
+          out Affordance affordanceSmoothed;
 
-    //instance Kalmanfilter kfLL;
-    //instance Kalmanfilter([predictions.distLL, 0]) kfLL;
-    //instance Kalmanfilter([predictions.distMM, 0]) kfMM;
-    //instance Kalmanfilter([predictions.distRR, 0]) kfRR;
-    //instance Kalmanfilter([predictions.distL, 0]) kfL;
-    //instance Kalmanfilter([predictions.distR, 0]) kfR;
+    instance KalmanFilterLL<2, 1> kfLL;
+    instance KalmanFilterML<2, 1> kfML;
+    instance KalmanFilterMM<2, 1> kfMM;
+    instance KalmanFilterMR<2, 1> kfMR;
+    instance KalmanFilterRR<2, 1> kfRR;
 
-    //connect predictions.angle -> predictionsSmoothed.angle;
-    //connect predictions.toMarkingLL -> predictionsSmoothed.toMarkingLL;
-    //connect predictions.toMarkingML -> predictionsSmoothed.toMarkingML;
-    //connect predictions.toMarkingMR -> predictionsSmoothed.toMarkingMR;
-    //connect predictions.toMarkingRR -> predictionsSmoothed.toMarkingRR;
-    //connect predictions.toMarkingL -> predictionsSmoothed.toMarkingL;
-    //connect predictions.toMarkingM -> predictionsSmoothed.toMarkingM;
-    //connect predictions.toMarkingR -> predictionsSmoothed.toMarkingR;
-
-    //connect predictions.distLL -> kfLL.measurement;
-    //connect predictions.distMM -> kfMM.measurement;
-    //connect predictions.distRR -> kfRR.measurement;
-    //connect predictions.distL -> kfL.measurement;
-    //connect predictions.distR -> kfR.measurement;
-
-    //connect kfLL.state-> predictionsSmoothed.distLL;
-    //connect kfMM.state-> predictionsSmoothed.distMM;
-    //connect kfRR.state-> predictionsSmoothed.distRR;
-    //connect kfL.state-> predictionsSmoothed.distL;
-    //connect kfR.state-> predictionsSmoothed.distR;
+    connect affordanceIn -> kfLL.affordanceIn;
+    connect kfLL.affordanceOut -> kfML.affordanceIn;
+    connect kfML.affordanceOut -> kfMM.affordanceIn;
+    connect kfMM.affordanceOut -> kfMR.affordanceIn;
+    connect kfMR.affordanceOut -> kfRR.affordanceIn;
+    connect kfRR.affordanceOut -> affordanceSmoothed;
 }
\ No newline at end of file

TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterLL.emadl

+package dp.subcomponents;
+
+component KalmanFilterLL<N nStates=2, nMeas=1> {
+    ports in Affordance affordanceIn,
+          out Affordance affordanceOut;
+
+    implementation Math {
+        Q^{nMeas, nMeas} dt = 0.1;
+        Q^{nMeas, nMeas} measurement = affordanceIn.distLL;
+        Q^{nStates, nStates} A = [1, dt; 0, 1]; // state transition matrix
+        Q^{nMeas, nStates} measM = [1, 0] ; // C: measurement matrix
+        Q^{nStates, nStates} procNoiseCov = [dt*dt*dt/3, dt*dt/2; dt*dt/2, dt]; // Q: covariance of process noise
+        Q^{nMeas, nMeas} measNoiseCov = 5; // R: covariance of measurement noise
+        Q^{nStates, nStates} errCov = [1000, 0; 0, 1000]; // P: estimate error covariance
+        Q^{nStates, nStates} I = ones(nStates, nStates);
+
+
+        // Prediction step
+        X = A * X;
+        errCov = A * errCov * trans(A) + procNoiseCov;
+
+        // Correction step
+        Q^{nStates,1} kalmanGain = errCov * trans(measM) * inverse(measM * errCov * trans(measM) + measNoiseCov);
+        X = X + kalmanGain * (measurement - measM * X);
+        errCov = (I - kalmanGain * errCov) * errorCovariance;
+        state = X(1, 1);
+        affordanceOut.distLL = state;
+
+    }
+}

TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterML.emadl

+package dp.subcomponents;
+
+component KalmanFilterML<N nStates=2, nMeas=1> {
+    ports in Affordance affordanceIn,
+          out Affordance affordanceOut;
+
+    implementation Math {
+        Q^{nMeas, nMeas} dt = 0.1;
+        Q^{nMeas, nMeas} measurement = affordanceIn.toMarkingML;
+        Q^{nStates, nStates} A = [1, dt; 0, 1]; // state transition matrix
+        Q^{nMeas, nStates} measM = [1, 0] ; // C: measurement matrix
+        Q^{nStates, nStates} procNoiseCov = [dt*dt*dt/3, dt*dt/2; dt*dt/2, dt]; // Q: covariance of process noise
+        Q^{nMeas, nMeas} measNoiseCov = 5; // R: covariance of measurement noise
+        Q^{nStates, nStates} errCov = [1000, 0; 0, 1000]; // P: estimate error covariance
+        Q^{nStates, nStates} I = ones(nStates, nStates);
+
+
+        // Prediction step
+        X = A * X;
+        errCov = A * errCov * trans(A) + procNoiseCov;
+
+        // Correction step
+        Q^{nStates,1} kalmanGain = errCov * trans(measM) * inverse(measM * errCov * trans(measM) + measNoiseCov);
+        X = X + kalmanGain * (measurement - measM * X);
+        errCov = (I - kalmanGain * errCov) * errorCovariance;
+        state = X(1, 1);
+        affordanceOut.toMarkingML = state;
+
+    }
+}

TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterMM.emadl

+package dp.subcomponents;
+
+component KalmanFilterMM<N nStates=2, nMeas=1> {
+    ports in Affordance affordanceIn,
+          out Affordance affordanceOut;
+
+    implementation Math {
+        Q^{nMeas, nMeas} dt = 0.1;
+        Q^{nMeas, nMeas} measurement = affordanceIn.distMM;
+        Q^{nStates, nStates} A = [1, dt; 0, 1]; // state transition matrix
+        Q^{nMeas, nStates} measM = [1, 0] ; // C: measurement matrix
+        Q^{nStates, nStates} procNoiseCov = [dt*dt*dt/3, dt*dt/2; dt*dt/2, dt]; // Q: covariance of process noise
+        Q^{nMeas, nMeas} measNoiseCov = 5; // R: covariance of measurement noise
+        Q^{nStates, nStates} errCov = [1000, 0; 0, 1000]; // P: estimate error covariance
+        Q^{nStates, nStates} I = ones(nStates, nStates);
+
+
+        // Prediction step
+        X = A * X;
+        errCov = A * errCov * trans(A) + procNoiseCov;
+
+        // Correction step
+        Q^{nStates,1} kalmanGain = errCov * trans(measM) * inverse(measM * errCov * trans(measM) + measNoiseCov);
+        X = X + kalmanGain * (measurement - measM * X);
+        errCov = (I - kalmanGain * errCov) * errorCovariance;
+        state = X(1, 1);
+        affordanceOut.distMM = state;
+
+    }
+}

TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterMR.emadl

+package dp.subcomponents;
+
+component KalmanFilterMR<N nStates=2, nMeas=1> {
+    ports in Affordance affordanceIn,
+          out Affordance affordanceOut;
+
+    implementation Math {
+        Q^{nMeas, nMeas} dt = 0.1;
+        Q^{nMeas, nMeas} measurement = affordanceIn.toMarkingMR;
+        Q^{nStates, nStates} A = [1, dt; 0, 1]; // state transition matrix
+        Q^{nMeas, nStates} measM = [1, 0] ; // C: measurement matrix
+        Q^{nStates, nStates} procNoiseCov = [dt*dt*dt/3, dt*dt/2; dt*dt/2, dt]; // Q: covariance of process noise
+        Q^{nMeas, nMeas} measNoiseCov = 5; // R: covariance of measurement noise
+        Q^{nStates, nStates} errCov = [1000, 0; 0, 1000]; // P: estimate error covariance
+        Q^{nStates, nStates} I = ones(nStates, nStates);
+
+
+        // Prediction step
+        X = A * X;
+        errCov = A * errCov * trans(A) + procNoiseCov;
+
+        // Correction step
+        Q^{nStates,1} kalmanGain = errCov * trans(measM) * inverse(measM * errCov * trans(measM) + measNoiseCov);
+        X = X + kalmanGain * (measurement - measM * X);
+        errCov = (I - kalmanGain * errCov) * errorCovariance;
+        state = X(1, 1);
+        affordanceOut.toMarkingMR = state;
+
+    }
+}

TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterRR.emadl

+package dp.subcomponents;
+
+component KalmanFilterRR<N nStates=2, nMeas=1> {
+    ports in Affordance affordanceIn,
+          out Affordance affordanceOut;
+
+    implementation Math {
+        Q^{nMeas, nMeas} dt = 0.1;
+        Q^{nMeas, nMeas} measurement = affordanceIn.distRR;
+        Q^{nStates, nStates} A = [1, dt; 0, 1]; // state transition matrix
+        Q^{nMeas, nStates} measM = [1, 0] ; // C: measurement matrix
+        Q^{nStates, nStates} procNoiseCov = [dt*dt*dt/3, dt*dt/2; dt*dt/2, dt]; // Q: covariance of process noise
+        Q^{nMeas, nMeas} measNoiseCov = 5; // R: covariance of measurement noise
+        Q^{nStates, nStates} errCov = [1000, 0; 0, 1000]; // P: estimate error covariance
+        Q^{nStates, nStates} I = ones(nStates, nStates);
+
+
+        // Prediction step
+        X = A * X;
+        errCov = A * errCov * trans(A) + procNoiseCov;
+
+        // Correction step
+        Q^{nStates,1} kalmanGain = errCov * trans(measM) * inverse(measM * errCov * trans(measM) + measNoiseCov);
+        X = X + kalmanGain * (measurement - measM * X);
+        errCov = (I - kalmanGain * errCov) * errorCovariance;
+        state = X(1, 1);
+        affordanceOut.distRR = state;
+
+    }
+}

TorcsEMAMGenerator/src/main/models/dp/subcomponents/Kalmanfilter.emadl

-package dp.subcomponents;
-
-component Kalmanfilter {
-    ports in Q^{1,1} measurement,
-          out Q^{1,2} state;
-
-    implementation Math {
-        //Q^{1, 1} dt = 0;
-        //Q^{2, 2} A = [1, dt; 0, 1]; // state transition matrix
-        //Q^{1, 2} measM = [1, 0] ; // C: measurement matrix
-        //Q^{2, 2} procNoiseCov = [dt*dt*dt/3, dt*dt/2; dt*dt/2, dt]; // Q: covariance of process noise
-        //Q^{1, 1} measNoiseCov = 5; // R: covariance of measurement noise
-        //Q^{2, 2} errCov = [1000, 0; 0, 1000]; // P: estimate error covariance
-        //Q^{2, 2} I = [1, 0; 0, 1];
-
-
-        // Prediction step
-        //X = A * X;
-        //errCov = A * errCov * trans(A) + procNoiseCov;
-
-        // Correction step
-        //Q^{2,1} kalmanGain = errCov * trans(measM) * inverse(measM * errCov * trans(measM) + measNoiseCov);
-        //X = X + kalmanGain * (measurement - measM * X);
-        //errCov = (I - kalmanGain * errCov) * errorCovariance;
-        //state = X(1, 1);
-    }
-}

TorcsEMAMGenerator/src/main/models/dp/subcomponents/SafetyNet.emam

+package dp.subcomponents;
+
+component SafetyNet {
+ports
+	in Q(0:255)^{3, 210, 280} data,
+	out Q(0:1) safetyLevel;
+
+	implementation Math {
+        safetyLevel=1.0;
+    }
+}

TorcsEMAMGenerator/src/main/models/dp/subcomponents/Safetycontroller.emam

-package dp.subcomponents;
-
-component Safetycontroller {
-ports
-	in Z(0:255)^{3, 210, 280} imageIn,
-	in Q(0:1)^{14} affordanceIn,
-	out Q(0:1) safetyLevelOut;
-
-	implementation Math {
-        safetyLevelOut=affordanceIn;
-    }
-}