/branches/KalmanFilter MikeW/kafi.c |
---|
172,10 → 172,6 |
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); |
735,4 → 731,5 |
/*--- (SYMBOLIC) CONSTANTS ---*/ |
/*--- VARIABLES ---*/ |
} |