Kalmanfilter.emadl 993 Bytes
Newer Older
1
2
package dp.subcomponents;

Svetlana's avatar
Draft    
Svetlana committed
3
component Kalmanfilter {
4
5
6
7
    ports in Q^{1,1} measurement,
          out Q^{1,2} state;

    implementation Math {
Svetlana's avatar
Svetlana committed
8
9
10
11
12
13
14
        //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];
15
16
17


        // Prediction step
Svetlana's avatar
Svetlana committed
18
19
        //X = A * X;
        //errCov = A * errCov * trans(A) + procNoiseCov;
20
21

        // Correction step
Svetlana's avatar
Svetlana committed
22
23
24
25
        //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);
26
27
    }
}