KalmanFilterMR.emadl 1.18 KB
Newer Older
Svetlana's avatar
Svetlana committed
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30
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;

    }
}