Skip to content

GitLab

Aufgrund einer Störung des s3 Storage, könnten in nächster Zeit folgende GitLab Funktionen nicht zur Verfügung stehen: Container Registry, Job Artifacs,. Wir bitten um Verständnis. Es wird mit Hochdruck an der Behebung des Problems gearbeitet. Weitere Informationen zur Störung des Object Storage finden Sie hier: https://maintenance.itc.rwth-aachen.de/ticket/status/messages/59-object-storage-pilot

Commit 2c4d88aa authored by Svetlana's avatar Svetlana
Browse files

Adjusted Kalman filters

parent 6f8e9b54
Hide whitespace changes
Inline Side-by-side
Showing with 182 additions and 76 deletions
TorcsEMAMGenerator/src/main/models/dp/Mastercomponent.emadl
View file @ 2c4d88aa
......@@ -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
View file @ 2c4d88aa
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
View file @ 2c4d88aa
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 0 → 100644
View file @ 2c4d88aa
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 0 → 100644
View file @ 2c4d88aa
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 0 → 100644
View file @ 2c4d88aa
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 0 → 100644
View file @ 2c4d88aa
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 0 → 100644
View file @ 2c4d88aa
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 deleted 100644 → 0
View file @ 6f8e9b54
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 0 → 100644
View file @ 2c4d88aa
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 deleted 100644 → 0
View file @ 6f8e9b54
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;
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment