Updated Kalman filter

EMADL2TORCS/src/main/dp/Mastercomponent.emadl

@@ -12,7 +12,7 @@ component Mastercomponent {
    instance Dpnet dpnet;
    instance DriverController driverController;
    instance Denormalizer denorm;
-   //instance KFMastercomponent kfm;
+   instance KFMastercomponent kfm;
    instance SteeringBuffer steeringBuffer;
    instance VectorToAffordance vecToAffordance;
    instance LocalizationController locController;
@@ -20,22 +20,21 @@ component Mastercomponent {
 
    connect imageIn -> dpnet.data;
    connect dpnet.predictions -> denorm.normalizedPredictions;
-
-   //connect denorm.affordance -> kfm.affordanceIn;
-   //connect kfm.affordanceSmoothed -> driverController.affordanceIn;
-   connect denorm.affordance -> driverController.affordanceIn, affToVector.affordanceIn;
+   connect denorm.affordance -> kfm.affordanceIn;
+   connect kfm.affordanceSmoothed -> locController.affordanceIn, driverController.affordanceIn, affToVector.affordanceIn;
+   connect locController.lanesCount -> driverController.lanesCountIn;
    connect affToVector.affordanceOut -> predictedAffordanceOut;
+   
    //connect groundTruthAffordance -> vecToAffordance.affordanceIndicators;
-   //connect vecToAffordance.affordance -> driverController.affordanceIn;
-   //connect vecToAffordance.affordance -> locController.affordanceIn;
+   //connect vecToAffordance.affordance -> driverController.affordanceIn, locController.affordanceIn;
 
    connect steeringBuffer.outputBuffer -> driverController.steeringRecordIn;
    connect speedIn -> driverController.speedIn;
-
-   connect locController.lanesCount -> driverController.lanesCountIn;
+   
    connect steeringBuffer.timerLeftOut -> driverController.timerLeftIn;
    connect steeringBuffer.timerRightOut -> driverController.timerRightIn;
    connect steeringBuffer.laneChangeOut -> driverController.laneChangeIn;
+
    connect driverController.commandsOut -> commandsOut;
    connect driverController.steerCmd -> steeringBuffer.inSteerCmd;
    connect driverController.timerLeftOut -> steeringBuffer.timerLeftIn;

EMADL2TORCS/src/main/dp/subcomponents/ConstantMatrix.emadl

+package dp.subcomponents;
+
+component ConstantMatrix<Z n=1, Z m=1>(Q^{n,m} value) {
+	port
+		out Q^{n,m} out1;
+    implementation Math{
+        out1=value;
+    }
+}

EMADL2TORCS/src/main/dp/subcomponents/KFMastercomponent.emadl

@@ -4,16 +4,91 @@ component KFMastercomponent {
     ports in Affordance affordanceIn,
           out Affordance affordanceSmoothed;
 
-    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 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
+    component SetSmoothedAffordance{
+	ports in Affordance affordanceIn,
+		in Q distLL,
+		in Q distMM,
+		in Q distRR,
+		in Q toMarkingML,
+		in Q toMarkingMR,
+		out Affordance affordanceOut;
+	implementation Math {
+         	affordanceOut = affordanceIn;
+		affordanceOut.distLL = distLL;
+		affordanceOut.distRR = distRR;
+		affordanceOut.toMarkingML = toMarkingML;
+		affordanceOut.toMarkingMR = toMarkingMR;
+	}
+    }
+    
+    // new measurement
+    instance ConstantMatrix<1, 1>([affordanceIn.distLL]) y_distLL;
+    instance ConstantMatrix<1, 1>([affordanceIn.distMM]) y_distMM;
+    instance ConstantMatrix<1, 1>([affordanceIn.distRR]) y_distRR;
+    instance ConstantMatrix<1, 1>([affordanceIn.toMarkingML]) y_toMarkingML;
+    instance ConstantMatrix<1, 1>([affordanceIn.toMarkingMR]) y_toMarkingMR;
+	
+    // 0.1 is a time Step between filter steps
+    instance ConstantMatrix<2, 2>([1, 0.1; 0, 1]) stateTrans;	// state transition matrix
+    instance ConstantMatrix<1, 2>([1, 0]) measM;		// C: measurement matrix
+    instance ConstantMatrix<2, 2>([0.1*0.1*0.1/3, 0.1*0.1/2; 0.1*0.1/2, 0.1]) procNoiseCov; 	 // Q: covariance of process noise
+    instance ConstantMatrix<1, 1>([5]) measNoiseCov;	// R: covariance of measurement noise
+
+    instance KalmanFilter kfLL;
+    instance KalmanFilter kfMM;
+    instance KalmanFilter kfRR;
+    instance KalmanFilter kfML;
+    instance KalmanFilter kfMR;
+ 
+    // instances to store updated X and error covariance
+    instance KalmanStatsBuffer(affordanceIn.distLL) statsLL;
+    instance KalmanStatsBuffer(affordanceIn.distMM) statsMM;
+    instance KalmanStatsBuffer(affordanceIn.distRR) statsRR;
+    instance KalmanStatsBuffer(affordanceIn.toMarkingML) statsML;
+    instance KalmanStatsBuffer(affordanceIn.toMarkingMR) statsMR;
+
+    instance SetSmoothedAffordance smoothedAffordance;
+    
+    connect affordanceIn 		-> smoothedAffordance.affordanceIn;
+    connect stateTrans.out1 		-> kfLL.stateTrans, kfMM.stateTrans, kfRR.stateTrans, kfML.stateTrans, kfMR.stateTrans;
+    connect measM.out1 			-> kfLL.measM, kfMM.measM, kfRR.measM, kfML.measM, kfMR.measM;
+    connect procNoiseCov.out1 		-> kfLL.procNoiseCov, kfMM.procNoiseCov, kfRR.procNoiseCov, kfML.procNoiseCov, kfMR.procNoiseCov;
+    connect measNoiseCov.out1		-> kfLL.measNoiseCov, kfMM.measNoiseCov, kfRR.measNoiseCov, kfML.measNoiseCov, kfMR.measNoiseCov;
+    
+    connect statsLL.outX 		-> kfLL.X;
+    connect statsLL.outErrCov 		-> kfLL.errCov;
+    connect y_distLL.out1 		-> kfLL.Y;
+    connect kfLL.correctedX 		-> statsLL.X;
+    connect kfLL.correctedErrCov	-> statsLL.errCov;
+    connect kfLL.predictedMeasurement	-> smoothedAffordance.distLL;
+
+    connect statsMM.outX 		-> kfMM.X;
+    connect statsMM.outErrCov 		-> kfMM.errCov;
+    connect y_distLL.out1 		-> kfMM.Y;
+    connect kfMM.correctedX 		-> statsMM.X;
+    connect kfMM.correctedErrCov 	-> statsMM.errCov;
+    connect kfMM.predictedMeasurement 	-> smoothedAffordance.distMM;
+
+    connect statsRR.outX 		-> kfRR.X;
+    connect statsRR.outErrCov		-> kfRR.errCov;
+    connect y_distRR.out1 		-> kfRR.Y;
+    connect kfRR.correctedX 		-> statsRR.X;
+    connect kfRR.correctedErrCov 	-> statsRR.errCov;
+    connect kfRR.predictedMeasurement 	-> smoothedAffordance.distRR;
+
+    connect statsML.outX 		-> kfML.X;
+    connect statsML.outErrCov 		-> kfML.errCov;
+    connect y_toMarkingML.out1 		-> kfML.Y;
+    connect kfML.correctedX 		-> statsML.X;
+    connect kfML.correctedErrCov 	-> statsML.errCov;
+    connect kfML.predictedMeasurement 	-> smoothedAffordance.toMarkingML;
+
+    connect statsMR.outX 		-> kfMR.X;
+    connect statsMR.outErrCov 		-> kfMR.errCov;
+    connect y_toMarkingMR.out1 		-> kfMR.Y;
+    connect kfMR.correctedX 		-> statsMR.X;
+    connect kfMR.correctedErrCov 	-> statsMR.errCov;
+    connect kfMR.predictedMeasurement 	-> smoothedAffordance.toMarkingMR;
+	
+    connect smoothedAffordance.affordanceOut -> affordanceSmoothed;
+}

EMADL2TORCS/src/main/dp/subcomponents/KalmanFilter.emadl

+package dp.subcomponents;
+
+component KalmanFilter {
+    ports 
+	in Q^{1, 2} X,
+	in Q^{1, 1} Y,
+	in Q^{2, 2} stateTrans,  // state transition matrix
+	in Q^{1, 2} measM,  // C: measurement matrix
+	in Q^{2, 2} procNoiseCov,  // Q: covariance of process noise
+	in Q^{1, 1} measNoiseCov, // R: covariance of measurement noise
+	in Q^{2, 2} errCov,// P: estimate error covariance
+	out Q^{1, 2} correctedX,
+	out Q^{2, 2} correctedErrCov,
+	out Q predictedMeasurement;
+
+    implementation Math {
+        Q^{2, 2} I = ones(2, 2);
+
+        // Prediction step
+        X = stateTrans * X;
+        errCov = stateTrans * errCov * trans(stateTrans) + procNoiseCov;
+
+        // Correction step
+        Q^{2,1} kalmanGain = errCov * trans(measM) * inv(measM * errCov * trans(measM) + measNoiseCov);
+        correctedX = X + kalmanGain * (Y - measM * X);
+        correctedErrCov = (I - kalmanGain * measM) * errCov;
+	predictedMeasurement = correctedX(1);
+	
+    }
+}

EMADL2TORCS/src/main/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;
-
-    }
-}

EMADL2TORCS/src/main/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;
-
-    }
-}

EMADL2TORCS/src/main/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;
-
-    }
-}

EMADL2TORCS/src/main/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;
-
-    }
-}

EMADL2TORCS/src/main/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;
-
-    }
-}

EMADL2TORCS/src/main/dp/subcomponents/KalmanStatsBuffer.emadl

+package dp.subcomponents;
+
+component KalmanStatsBuffer(Q startValue) {
+    ports in Q^{1, 2} X,
+	  in Q^{2, 2} errCov,
+          out Q^{1, 2} outX,
+	  out Q^{2, 2} outErrCov;
+
+    implementation Math {
+            static Q^{1, 2} bufferedX = [startValue, 0];
+            static Q^{2, 2} bufferedErrCov = [1000, 0; 0, 1000];
+
+	    outX = bufferedX;
+	    outErrCov = bufferedErrCov;
+
+	    bufferedX = X;
+	    bufferedErrCov = errCov;
+        }
+}