diff --git a/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl b/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl index 2e6f349f43b930cda9cb4b0f12aa4c1e2198c266..14bd68bd352b0868882d98eddbac65a879a0eef7 100644 --- a/TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl +++ b/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; diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/DriverController.emadl similarity index 93% rename from TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl rename to TorcsEMAMGenerator/src/main/models/dp/subcomponents/DriverController.emadl index db4c2f469a41bbfc12833318d852e425e0ef67a2..4f4e87c5481d623ba727ba71032df5d7b269f2f7 100644 --- a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Drivercontroller.emadl +++ b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/DriverController.emadl @@ -1,6 +1,6 @@ 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 diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KFMastercomponent.emadl b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KFMastercomponent.emadl index b606d0b14d49ec2d51598f1fe23f187ccaec0966..d24b520eb08ee723f3bae0ccdd2282374261f2e9 100644 --- a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KFMastercomponent.emadl +++ b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KFMastercomponent.emadl @@ -1,34 +1,19 @@ 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 diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterLL.emadl b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterLL.emadl new file mode 100644 index 0000000000000000000000000000000000000000..5109abd3a8aacee35c4fad2e65bd605a7ca498d9 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterLL.emadl @@ -0,0 +1,30 @@ +package dp.subcomponents; + +component KalmanFilterLL { + 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; + + } +} diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterML.emadl b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterML.emadl new file mode 100644 index 0000000000000000000000000000000000000000..31527f42f352306aa2d98c996e697c2be74ed767 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterML.emadl @@ -0,0 +1,30 @@ +package dp.subcomponents; + +component KalmanFilterML { + 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; + + } +} diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterMM.emadl b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterMM.emadl new file mode 100644 index 0000000000000000000000000000000000000000..66074965b2953586079ebbed6a15f4381ba7ecf1 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterMM.emadl @@ -0,0 +1,30 @@ +package dp.subcomponents; + +component KalmanFilterMM { + 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; + + } +} diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterMR.emadl b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterMR.emadl new file mode 100644 index 0000000000000000000000000000000000000000..035db743a4074e2e596286522683cae727083e66 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterMR.emadl @@ -0,0 +1,30 @@ +package dp.subcomponents; + +component KalmanFilterMR { + 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; + + } +} diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterRR.emadl b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterRR.emadl new file mode 100644 index 0000000000000000000000000000000000000000..95a331e34e0109d4f054f1a8b76e3c3ee0384643 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/KalmanFilterRR.emadl @@ -0,0 +1,30 @@ +package dp.subcomponents; + +component KalmanFilterRR { + 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; + + } +} diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Kalmanfilter.emadl b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Kalmanfilter.emadl deleted file mode 100644 index a40fbaf99b0864116c320f17ed7a9b13d4fa72f5..0000000000000000000000000000000000000000 --- a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Kalmanfilter.emadl +++ /dev/null @@ -1,27 +0,0 @@ -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); - } -} diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/SafetyNet.emam b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/SafetyNet.emam new file mode 100644 index 0000000000000000000000000000000000000000..085761529a67db99c7c14a4419699346f23e9334 --- /dev/null +++ b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/SafetyNet.emam @@ -0,0 +1,11 @@ +package dp.subcomponents; + +component SafetyNet { +ports + in Q(0:255)^{3, 210, 280} data, + out Q(0:1) safetyLevel; + + implementation Math { + safetyLevel=1.0; + } +} diff --git a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Safetycontroller.emam b/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Safetycontroller.emam deleted file mode 100644 index 892e4b97c5d79465e572b19677fbd965c7a5d079..0000000000000000000000000000000000000000 --- a/TorcsEMAMGenerator/src/main/models/dp/subcomponents/Safetycontroller.emam +++ /dev/null @@ -1,12 +0,0 @@ -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; - } -}