Rev 973 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 973 | Rev 982 | ||
|---|---|---|---|
| Line 170... | Line 170... | ||
| 170 | matSetDiagonal(matQ, _Phi, _Phi, 0.00001F); |
170 | matSetDiagonal(matQ, _Phi, _Phi, 0.00001F); |
| 171 | matSetDiagonal(matQ, _Theta, _Theta, 0.00001F); |
171 | matSetDiagonal(matQ, _Theta, _Theta, 0.00001F); |
| 172 | matSetDiagonal(matQ, _Psi, _Psi, 0.00005F); |
172 | matSetDiagonal(matQ, _Psi, _Psi, 0.00005F); |
| 173 | #endif |
173 | #endif |
| Line -... | Line 174... | ||
| - | 174 | ||
| - | 175 | matSetDiagonal(matQ, _Phi, _Phi, 0.000006F); |
|
| - | 176 | matSetDiagonal(matQ, _Theta, _Theta, 0.000006F); |
|
| - | 177 | matSetDiagonal(matQ, _Psi, _Psi, 0.000006F); |
|
| 174 | 178 | ||
| 175 | /* set diagonal init. estimation error (covariance) P0 */ |
179 | /* set diagonal init. estimation error (covariance) P0 */ |
| 176 | matSetDiagonal(matPs, _Phi, _Phi, 0.00001F); |
180 | matSetDiagonal(matPs, _Phi, _Phi, 0.00001F); |
| 177 | matSetDiagonal(matPs, _Theta, _Theta, 0.00001F); |
181 | matSetDiagonal(matPs, _Theta, _Theta, 0.00001F); |
| Line 729... | Line 733... | ||
| 729 | void trEstimateVelocity() |
733 | void trEstimateVelocity() |
| 730 | { |
734 | { |
| 731 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
735 | /*--- (SYMBOLIC) CONSTANTS ---*/ |
| Line 732... | Line 736... | ||
| 732 | 736 | ||
| 733 | /*--- VARIABLES ---*/ |
- | |
| 734 | 737 | /*--- VARIABLES ---*/ |
|