24d8cc62e2
AP_NavEKF2: rework yaw and magnetic heading reset logic
...
Splits in-flight yaw alignment completed status into separate yaw and magnetic field flags.
Reduce the number of places where decisions to perform a yaw and field reset are made.
Don't perform a reset unless there is is data in the buffer
Don't use 3-axis fusion if the field states still need to be reset.
When starting 3-axis fusion request a reset if not previously performed.
Ensure magnetometer and GPs heading resets are alwasy perfomred with data at teh correct time horizon.
2016-05-31 16:12:53 +10:00
a94a19bd09
AP_NavEKF2: Fix bug causing continual yaw resets
2016-05-28 06:52:11 +10:00
97e2203e70
AP_NavEKF2: don't do 3D mag fusion on 2nd EKF2 core
...
this reduces the risk that mag fusion errors will badly affect
attitude estimation.
2016-05-27 21:27:06 +10:00
de15928921
AP_NavEKF2: prevent potential arithmetic error in declination fusion
2016-05-27 09:00:41 +10:00
722eb0d706
AP_NavEKF2: Fix bug for start of wind speed estimation
...
The commencement of airspeed fusion could cause pitch errors due to small variances and large innovations. This issue is addressed by the following changes:
1) The airspeed measurement is used to set wind states to a value that reduces initial innovations.
2) The wind state variances are set to values that better reflect the wind speed uncertainty
2016-05-27 09:00:41 +10:00
ac329ec31c
AP_NavEKF2: use observation noise to set initial magnetic field variances
2016-05-27 09:00:41 +10:00
830751c0ae
AP_NavEKF2: remove un-wanted functionality from mag and yaw reset
...
The function used to reset magnetic field states and yaw angle should not be used when there is no magnetometer. If it is incorrectly called without a magnetometer it should not change the attitude or field states.
2016-05-27 09:00:41 +10:00
11c6ea7ef6
AP_NavEKF2: attitude co-variance reset for all post alignment yaw resets
...
Whenever the yaw angle is changed, the correlation with other state errors will be incorrect and the terms should be zeroed.
2016-05-27 09:00:41 +10:00
cdd09df9ac
AP_NavEKF: Add function to zero attitude state co-variances
...
When changing the vehicle yaw angle, the correlation between the attitude errors and errors in other states is invalid so the corresponding co-variance terms need to be zeroed.
This needs to be done in more than one place.
2016-05-27 09:00:41 +10:00
6d34ac5ceb
AP_NavEKF2: improve switch-over to use of mag field states
...
Set initial variances to measurement uncertainties
Always perform a field and yaw reset
2016-05-27 09:00:40 +10:00
fe9ddfdfeb
AP_NavEKF2: remove code that prevents attitude updates after mag reset
...
This code assumes a vehicle is close to level and will not work for all vehicle types. It will be replaced by a different method.
2016-05-27 09:00:40 +10:00
fe85c68344
AP_NavEKF2: move yaw reset for no compass case
...
Should not be in a function that performs reset on magnetic field states
2016-05-27 09:00:40 +10:00
da0622827d
AP_NavEKF: update tuning defaults
...
Increase speed of scale factor learning
2016-05-27 09:00:40 +10:00
9eef97d108
AP_NavEKF2: use consistent parameter naming
...
M_NSE is a measurement noise
P_NSE is a observation noise
I_GATE is an innovation gate
This also ensures the new parameter values required to use the EKF2 will be enforced.
2016-05-25 12:06:04 +10:00
74121fdaeb
AP_NavEKF2: fix bug preventing in-flight alignment
2016-05-21 15:13:53 +10:00
16cb7078cd
AP_NavEKF2: Fix misleading pre-flight status reporting
2016-05-21 15:13:53 +10:00
38d61f5281
AP_NavEKF2: Update status reporting to handle no-magnetometer startup
2016-05-21 15:13:53 +10:00
9aa3cdfaae
AP_NavEKF2: Update GPS yaw re-alignment function
...
Updates to support initial alignment without magnetometer
2016-05-21 15:13:53 +10:00
e3c966661b
AP_NavEKF2: Remove unnecessary variance reset
...
This reset is unnecessary give that a synthetic yaw angle is fused at a low rate to prevent variances from becoming badly conditioned.
2016-05-21 15:13:53 +10:00
1179c08473
AP_NavEKF2: Handle case where we are in-flight and haven't yet aligned the yaw
...
This happens if planes are flown without a compass
2016-05-21 15:13:53 +10:00
581c1aa0d4
AP_NavEKF2: Adjust attitude variance values used after a heading reset
...
Because these errors are in body frame, a spherical error should be used to accommodate different orientations
2016-05-21 15:13:53 +10:00
5ec8d523f6
AP_NavEKF: remove unwanted co-variance reset
...
The co-variance resets required following a velocity and position state reset is now performed in the stte reset function
2016-05-21 15:13:53 +10:00
66e4d9d6e8
AP_NavEKF2: Reset co-variance values when doing a pos,vel,hgt reset
...
Ensures consistent behaviour after resets and reduces attitude disturbances
2016-05-21 15:13:52 +10:00
cf8175a073
AP_NavEKF2: Allow immediate use of GPS in-flight for plane without compass
...
Planes operating without a compass must align the yaw to the GPS heading immediately after launch or takeoff and cannot wait for GPS accuracy checks.
2016-05-21 15:13:52 +10:00
08de4e6d95
AP_NavEKF: Do not perform start-up yaw alignment if no compass
...
Yaw cannot be aligned until in-flight when there is sufficient GPS velocity.
2016-05-21 15:13:52 +10:00
d2694fe5dc
AP_NavEKF2: Remove duplicate transition into GPS aiding
...
This decision is made in one place in setAidingMode()
2016-05-21 15:13:52 +10:00
7201f7d2fd
AP_NavEKF2: fix comment
2016-05-21 15:13:52 +10:00
64a8153b68
AP_NavEKF2: update function name
...
The primary purpose of this function is re-alignment
2016-05-21 15:13:52 +10:00
addd213af0
AP_NavEKF2: reduce effect of acceleration on non-GPS mode attitude
2016-05-21 15:13:52 +10:00
a3b78e6231
AP_NavEKF2: update tuning defaults
2016-05-21 15:13:51 +10:00
65f63341d7
AP_NavEKF2: Improve numerical error protection in optical flow fusion
...
Abort fusion of data if variances will become negative and publish the fault
2016-05-21 15:13:51 +10:00
42da33593d
AP_NavEKF2: fix yaw fusion numerical health reporting
2016-05-21 15:13:51 +10:00
6eb9d43507
AP_NavEKF2: add numerical error checking and isolation to mag decl fusion
2016-05-21 15:13:51 +10:00
b325c5faeb
AP_NavEKF2: add error checking and isolation to mag heading fusion
2016-05-21 15:13:51 +10:00
a0b1dc5325
AP_NavEKF2: add error checking and isolation to 3D amg fusion
2016-05-21 15:13:51 +10:00
51a3df0319
AP_NavEKF2: Add error checking and isolation to pos vel fusion
2016-05-21 15:13:51 +10:00
71b589c89c
AP_NavEKF2: extend fusion fault reporting coverage
2016-05-21 15:13:50 +10:00
0982bd0b2b
AP_NavEKF2: update parameter documentation
2016-05-21 15:13:50 +10:00
46a2993a0d
AP_NavEKF: use intuitive units for imu bias process noise parameters
2016-05-21 15:13:50 +10:00
8c374fd679
AP_NavEKF2: correct comments
2016-05-21 15:13:50 +10:00
6be9eaa524
AP_NavEKF2: use receiver estimated accuracy
...
Adjust the GPS observation noise based on receiver accuracy output if available.
2016-05-21 15:13:50 +10:00
dddc213836
AP_NavEKF2: Open up tuning limits
...
Done to assist tuning assessments
2016-05-21 15:13:50 +10:00
cf05e110fc
AP_NavEKF2: fix scaling error in use of IMU noise parameters
...
Also remove process noise from state transition matrix where it was mistakenly left in after the derivation
2016-05-21 15:13:50 +10:00
4080c6091d
AP_NavEKF2: fixed an assumption that _ahrs->get_compass() always works
...
if MAG_ENABLE=0 in plane there is no compass
2016-05-19 14:16:40 +10:00
d67b1edf10
AP_NavEKF2: (potential) compiler warning of float to double promotion
2016-05-17 20:30:51 -07:00
64d14356b9
AP_NavEKF2: Fix typos
2016-05-13 19:20:06 -03:00
41661f815f
AP_Math: Replace the pythagorous* functions with a variadic template
...
The new function can deal with a variable number of function parameters.
Additionally, I renamed the functions to norm(), because this is the
standard name used in several other projects.
2016-05-10 11:41:26 -03:00
ea508f1b80
AP_NavEKF2: use LOG_REPLAY and EK2_LOG_MASK parameters
2016-05-09 12:26:57 +10:00
32af886ba9
AP_NavEKF2: use consistent logging timestamps for sensors
2016-05-07 18:27:22 +10:00
4e4c575f16
AP_NavEKF2: added have_ekf_logging()
2016-05-07 18:27:20 +10:00
Paul Riseborough
Andrew Tridgell
Tom Pittenger
Ricardo de Almeida Gonzaga
dgrat