KFMastercomponent.emadl 3.94 KB
Newer Older
1 2 3
package dp.subcomponents;

component KFMastercomponent {
Svetlana's avatar
Svetlana committed
4 5
    ports in Affordance affordanceIn,
          out Affordance affordanceSmoothed;
6

Svetlana's avatar
Svetlana committed
7
    component AffordanceUpdater{
Svetlana Pavlitskaya's avatar
Svetlana Pavlitskaya committed
8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49
	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;

Svetlana's avatar
Svetlana committed
50
    instance AffordanceUpdater affordanceUpdater;
Svetlana Pavlitskaya's avatar
Svetlana Pavlitskaya committed
51
    
Svetlana's avatar
Svetlana committed
52
    connect affordanceIn 		-> affordanceUpdater.affordanceIn;
Svetlana Pavlitskaya's avatar
Svetlana Pavlitskaya committed
53 54 55 56 57 58 59 60 61 62
    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;
Svetlana's avatar
Svetlana committed
63
    connect kfLL.predictedMeasurement	-> affordanceUpdater.distLL;
Svetlana Pavlitskaya's avatar
Svetlana Pavlitskaya committed
64 65 66 67 68 69

    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;
Svetlana's avatar
Svetlana committed
70
    connect kfMM.predictedMeasurement 	-> affordanceUpdater.distMM;
Svetlana Pavlitskaya's avatar
Svetlana Pavlitskaya committed
71 72 73 74 75 76

    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;
Svetlana's avatar
Svetlana committed
77
    connect kfRR.predictedMeasurement 	-> affordanceUpdater.distRR;
Svetlana Pavlitskaya's avatar
Svetlana Pavlitskaya committed
78 79 80 81 82 83

    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;
Svetlana's avatar
Svetlana committed
84
    connect kfML.predictedMeasurement 	-> affordanceUpdater.toMarkingML;
Svetlana Pavlitskaya's avatar
Svetlana Pavlitskaya committed
85 86 87 88 89 90

    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;
Svetlana's avatar
Svetlana committed
91
    connect kfMR.predictedMeasurement 	-> affordanceUpdater.toMarkingMR;
Svetlana Pavlitskaya's avatar
Svetlana Pavlitskaya committed
92
	
Svetlana's avatar
Svetlana committed
93
    connect affordanceUpdater.affordanceOut -> affordanceSmoothed;
Svetlana Pavlitskaya's avatar
Svetlana Pavlitskaya committed
94
}