bresch
b0f79caf34
GPS yaw: Extract and refactor GPS yaw control logic
...
- Rename GPS yaw fusion functions:
resetGpsAntYaw -> resetYawToGps
fuseGpsAntYaw -> fuseGpsYaw
- Move last fusion time in reset function
2020-07-02 13:35:13 +02:00
bresch
7f4fedde6a
GPS Yaw: fix heading initialisation and add unit tests
...
When the antennas are not parallel to the x body axis, the GPS message
contains the angular offset but the data is already corrected in the
driver. EKF2 should then not add this offset during the initialisation.
2020-07-02 13:35:13 +02:00
kamilritz
16a00eae0b
Make Kfusion a Vector24f
2020-07-02 09:02:24 +02:00
kamilritz
356b2f6ffe
Stop setting zero to zero
2020-07-02 09:02:24 +02:00
kamilritz
552bf824ea
Add typedef for Matrix 24 types
2020-07-02 09:02:24 +02:00
kamilritz
c3653e68ca
Add const modifier
2020-06-26 08:26:26 +02:00
kamilritz
48f0eb16da
Remove uninformative comments
2020-06-26 08:26:26 +02:00
kamilritz
4a69b41015
Increase matrix library usage even more
2020-06-26 08:26:26 +02:00
kamilritz
b8f937666a
Make mag_innov/-var a Matrix::Vector3f
2020-06-26 08:26:26 +02:00
bresch
4b746a3fca
GPS Yaw: add consts and remove fusion starting message
...
The fusion status is captured in the status flags; the message is
superfluous, uses flash space and can impact real-time performance
2020-06-23 08:43:48 +02:00
bresch
fe2a9d3018
GPS Yaw: move isfinite checks in control.cpp
2020-06-23 08:43:48 +02:00
bresch
3c6790f5d5
GPS Yaw: Always reset Yaw when GPS Yaw fusion is starting
...
Also avoid fusing fake mag data when an other source of yaw aiding
is active, even if in MAG_TYPE_NONE mode.
2020-06-23 08:43:48 +02:00
Daniel Agar
600240d95f
EKF: resetGpsAntYaw() fix double promotion
2020-05-20 12:44:18 -04:00
Kamil Ritz
21f49c22dc
Use resetQuatStateYaw during gps yaw reset
2020-05-20 08:52:52 +02:00
kritz
24f2e60b7e
Reduce stored strings, to save flash space ( #815 )
2020-05-13 08:09:26 +02:00
bresch
edc8a88c24
yaw_fusion: remove useless initialization
2020-05-08 16:55:23 +02:00
bresch
81c6d6655f
ekf: clean uncorrelateQuatStates function
...
As the name can be ambiguous, it gets renamed
"uncorrelateQuatFromOtherStates".
Also replace the loops storing the values and reapplying them by simply
zeroing two slices (this also saves 130B of flash).
2020-02-13 14:33:32 +01:00
kamilritz
4b0e7b2cff
Remove filtering on external vision alignment
2020-01-24 08:23:12 +01:00
kamilritz
3daf25763e
Remove unnecessary comments
2020-01-08 09:00:31 +01:00
kamilritz
48787c0160
Improve Matrix library usage
2020-01-08 09:00:31 +01:00
kamilritz
9e6d27fafb
Add missing const qualifier
2020-01-08 09:00:31 +01:00
Paul Riseborough
0d0f46ec1c
EKF: Don't run unnecessary makeRowColSymmetric operation
...
This operation is expensive when done to the whole covariance matrix and unnecessary after covariance prediction because we calculate the upper diagonal and copy across so it is already symmetric.
2020-01-02 19:26:57 +11:00
kamilritz
92ba618f57
Improve on flash memory usage
2020-01-02 19:26:57 +11:00
kamilritz
459b76f9fd
Make covariance matrices of type matrix::SquareMatrix
...
and use functions from Matrix to reset covariances.
2020-01-02 19:26:57 +11:00
kamilritz
1bf09fd370
Remove EKF prefix from logged messages
2019-12-12 08:48:10 +01:00
Paul Riseborough
36de2b3dc1
EKF: Use matrix library for quaternion to rotation matrix conversion
2019-10-23 14:44:08 +03:00
kritz
4a38f5a2f6
Add timestamp to ECL print statements, ( #644 )
...
which is helpful for EKF replay debugging.
2019-09-25 11:24:42 +02:00
Paul Riseborough
a036cf82cc
EKF: Remove use of of quaternion self product operator and fix delta rotation sign
2019-06-17 19:24:55 +10:00
Carl Olsson
f0889c1760
EKF: fixed some comment typos
...
Signed-off-by: CarlOlsson <carlolsson.co@gmail.com >
2019-03-18 10:20:33 -04:00
Paul Riseborough
ef5a87c1d4
EKF: Rework quaternion yaw reset.
...
Use a new method that preserves the roll and pitch information and adds the uncertainty for yaw only.
Ensure that correlation information to non-quaternion states is removed when a reset occurs to prevent fusion of subsequent observations (e.g. GPS) causing incorrect yaw.
2019-01-31 09:53:57 -05:00
Paul Riseborough
0220f469b0
EKF: Use more generic variable name for bad yaw fusion flag
...
This flag now reports on fusion of data that is not from a magnetometer.
2018-09-19 06:32:43 +10:00
Paul Riseborough
eb52f7c410
EKF: Remove implicit conversion and use single precision trig operator
...
Double precision accuracy is not required for this operation.
2018-09-19 06:32:43 +10:00
Paul Riseborough
d1f3f4c916
EKF: Use #define function for isfinite check
2018-09-19 06:32:43 +10:00
Paul Riseborough
91f886cb5e
EKF: Add support for use of GPS heading data.
...
Heading data is assumed to be from a dual antenna array at a specified yaw angle offset in body frame, but with the heading data already corrected for antenna offset. The offset is required to apply the correct compensation for combined rotations and to determine when the yaw observation has become badly conditioned.
2018-09-19 06:32:43 +10:00
bresch