Skip to content
GitLab
Commit bd1c3032 authored by Svetlana Pavlitskaya's avatar Svetlana Pavlitskaya
Browse files

Updated Kalman filter

parent 28fc37d5
Hide whitespace changes
Inline Side-by-side
EMADL2TORCS/src/main/dp/Mastercomponent.emadl
View file @ bd1c3032
......@@ -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 0 → 100644
View file @ bd1c3032
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
View file @ bd1c3032
......@@ -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 0 → 100644
View file @ bd1c3032
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 deleted 100644 → 0
View file @ 28fc37d5
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 deleted 100644 → 0
View file @ 28fc37d5
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 deleted 100644 → 0
View file @ 28fc37d5
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 deleted 100644 → 0
View file @ 28fc37d5
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 deleted 100644 → 0
View file @ 28fc37d5
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 0 → 100644
View file @ bd1c3032
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;
}
}
Supports Markdown
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