/*------------------------------------------*\
     |  Kalman model                              |
     |                                            |
     |  state quation                             |
     |  x(k) = A.x(k-1)+B.u(k)+w(k-1)             |
     |                                            |
     |  observations equation                     |
     |  z(k) = H.x(k)+y(k)                        |
     |                                            |
     |  prediction equations                      |
     |  x^(k) = A.x^(k-1) + B.u(k)                |
     |  P^(k) = A.P(k-1).A^T + Q                  |
     |                                            |
     |  correction equations                      |
     |  K(k) = P^(k).H^T . (H.P^(k).H^T + R)^-1   |
     |  x(k) = x^(k) + K(k).(z(k) - H*x^(k))      |
     |  P(k) = (I - K(k).H).P^(k)                 |
     |                                            |
     \*------------------------------------------*/

   // Kalman filter with order 2x2
    self.kalmanFilter= [MGKalman filterWithStateOrder:stateOrder observationOrder:observationOrder];
    
    
    // A matrix
    [_kalmanFilter setA:[MGMatrix identity:stateOrder]];
    
    /* H matrix */
    [_kalmanFilter setH:[MGMatrix rows:observationOrder columns:stateOrder values:
                         1.0,   0.0,
                         0.0,   1.0
                         ]];
    
    /* B Matrix */
    [_kalmanFilter setB:[MGMatrix rows:stateOrder columns:stateOrder]];
    
    // Q Matrix
    [_kalmanFilter setQ:[MGMatrix rows:stateOrder columns:stateOrder]];
    
    // R Matrix
    [_kalmanFilter setR:[MGMatrix rows:observationOrder columns:observationOrder values:
                         1.0,  0.0,
                         0.0,   1.
                         ]];
    
    
    // Po Matrix
    [_kalmanFilter setP_estimated:[MGMatrix rows:stateOrder columns:stateOrder values:
                                   1000.0,   0.0,
                                   0.0,   1000.0
                                   ]];
    
    // X(0)
    [_kalmanFilter setX_estimated:[MGMatrix rows:stateOrder columns:1 values:

    [_kalmanFilter estimateWithNewObservation:[MGMatrix rows:2 columns:1 values:
                                               z1,
                                               z2
                                               ]];