/branches/KalmanFilter MikeW/kafi.c |
---|
172,6 → 172,10 |
matSetDiagonal(matQ, _Psi, _Psi, 0.00005F); |
#endif |
matSetDiagonal(matQ, _Phi, _Phi, 0.000006F); |
matSetDiagonal(matQ, _Theta, _Theta, 0.000006F); |
matSetDiagonal(matQ, _Psi, _Psi, 0.000006F); |
/* set diagonal init. estimation error (covariance) P0 */ |
matSetDiagonal(matPs, _Phi, _Phi, 0.00001F); |
matSetDiagonal(matPs, _Theta, _Theta, 0.00001F); |
731,5 → 735,4 |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
/*--- VARIABLES ---*/ |
} |