Commit 07d1ed69 authored by Svetlana's avatar Svetlana

Further changes in driver controller, KalmanFilters

parent a0f0db52

Too many changes to show.

To preserve performance only 1000 of 1000+ files are displayed.

......@@ -10,10 +10,14 @@ component Mastercomponent {
instance Imagepreprocessing ip;
instance Dpnet dpnet;
instance Drivercontroller dc;
instance Unnormalizer un;
instance KFMastercomponent kfm;
connect imageIn -> ip.imageIn;
connect ip.imageOut -> dpnet.image;
connect dpnet.predictions -> dc.affordanceIn;
connect dpnet.predictions -> un.normalizedPredictions;
connect un.affordance -> kfm.predictions;
connect kfm.predictionsSmoothed -> dc.affordanceIn;
connect speedIn -> dc.speedIn;
connect dc.commandsOut -> commandsOut;
}
\ No newline at end of file
package dp.subcomponents;
struct Affordance {
Q(-0.5rad:0.001rad:0.5rad) angle;
Q(-9.5m:0.01m:-4m) toMarkingLL;
Q(-5.5m:0.01m:-0.5m) toMarkingML;
Q(0.5m:0.01m:5.5m) toMarkingMR;
Q(4m:0.01m:9.5m) toMarkingRR;
Q(0m:0.1m:75m) distLL;
Q(0m:0.1m:75m) distMM;
Q(0m:0.1m:75m) distRR;
Q(-7m:0.01m:-2.5m) toMarkingL;
Q(-2m:0.01m:3.5m) toMarkingM;
Q(2.5m:0.01m:7m) toMarkingR;
Q(0m:0.1m:75m) distL;
Q(0m:0.1m:75m) distR;
}
\ No newline at end of file
package dp.subcomponents;
struct Commands {
Q(0:1) acceleration;
Q(0:1) brake;
Q(-1:1) steering;
}
......@@ -2,16 +2,16 @@ package dp.subcomponents;
component Drivercontroller {
ports
in Q(0:1)^{1,1,13} affordanceIn,
in Q speedIn,
in Affordance affordanceIn,
in Q(0 m/s:0.1 m/s:100 m/s) speedIn,
out Q(0:1)^{3,1,1} commandsOut;
implementation Math {
Q Constant = 1;
Q angle = affordanceIn(0,0,0);
Q roadWidth = affordanceIn(0,0,1) + affordanceIn(0,0,4);
Q laneWidth = affordanceIn(0,0,2) + affordanceIn(0,0,3);
Q distToLaneCenter = abs(laneWidth/2 - affordanceIn(0,0,2));
//Q Constant = 1;
//Q angle = affordanceIn.angle;
//Q roadWidth = affordanceIn.toMarkingLL + affordanceIn.toMarkingRR;
//Q laneWidth = affordanceIn(0,0,2) + affordanceIn(0,0,3);
//Q distToLaneCenter = abs(laneWidth/2 - affordanceIn(0,0,2));
// Steering
commandsOut(2,1,1) = 0;//Constant * (angle-distToLaneCenter/roadWidth); // steering
......@@ -22,23 +22,25 @@ component Drivercontroller {
Q vD = 0;
Q desiredSpeed = 15;//vMax * (exp(-(vC/vMax) * affordanceIn(0,0,6) - vD));
Q accelCmd = 0.1 * (desiredSpeed - speedIn + 1);
if accelCmd > 1
accelCmd = 1.0;
commandsOut(2,1,1) = 0;
Q(0:1) acceleration = 0.1 * (desiredSpeed - speedIn + 1);
if acceleration > 1
acceleration = 1.0;
end
Q brakeCmd = 0.1 * (speedIn - desiredSpeed);
if brakeCmd > 1
brakeCmd = 1;
Q(0:1) brake = 0.1 * (speedIn - desiredSpeed);
if brake > 1
brake = 1;
end
// acceleration and brake
if speedIn < desiredSpeed
commandsOut(1,1,1) = accelCmd;
commandsOut(1,1,1) = acceleration;
commandsOut(3,1,1) = 0;
else
commandsOut(1,1,1) = 0;
commandsOut(3,1,1) = brakeCmd;
commandsOut(3,1,1) = brake;
end
}
}
\ No newline at end of file
package dp.subcomponents;
component KFMastercomponent {
ports in Affordance predictions,
out Affordance predictionsSmoothed;
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;
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;
}
\ No newline at end of file
package dp.subcomponents;
component Kalmanfilter(Q^{1, 2} X) {
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*dt2/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);
}
}
package dp.subcomponents;
component Unnormalizer {
ports in Q^{13,1,1} normalizedPredictions,
out Affordance affordance;
implementation Math {
affordance.angle = normalizedPredictions(1,1,1) - 0.5;
affordance.toMarkingLL = normalizedPredictions(2,1,1) * 4.5 - 7;
affordance.toMarkingML = normalizedPredictions(3,1,1) * 5.5 - 2;
affordance.toMarkingMR = normalizedPredictions(4,1,1) * 4.5 + 2.5;
affordance.toMarkingRR = normalizedPredictions(5,1,1) * 5.5 - 9.5;
affordance.distLL = normalizedPredictions(6,1,1) * 5 - 5.5;
affordance.distMM = normalizedPredictions(7,1,1) * 5 - 0.5;
affordance.distRR = normalizedPredictions(8,1,1) * 5.5 + 4;
affordance.toMarkingL = normalizedPredictions(9,1,1) * 75;
affordance.toMarkingM = normalizedPredictions(10,1,1) * 75;
affordance.toMarkingR = normalizedPredictions(11,1,1) * 75;
affordance.distL = normalizedPredictions(12,1,1) * 75;
affordance.distR = normalizedPredictions(13,1,1) * 75;
}
}
\ No newline at end of file