| /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 ---*/ |
| } |