5.4 Design of Extended Kalman Filters 89
90 5 Measurement Signal Enhancement
5.4.1 EKF for AHRS with Accelerometer Measurement
Based on the AHRS dynamics model with the accelerometer measurement as given by (5.23) and (5.24), we present the detailed EKF parameters for the accelerometer- based AHRS signal enhancement below. We should note that we have used the result of [87] as the initial baseline of our design.
1. Initial setting: Initialization is required for the state vector and error covariance matrix. The numerical values adopted for the EKF of the accelerometer-based AHRS are respectively given as
xA,0=[ 1 0 0 0 0 0 0 ]T (5.36)
and
PA,0=diag
0.1,0.1,0.1,0.1,0.1,0.1,0.1
. (5.37)
2. Noise covariance matrices setting: Qcovand Rcovare respectively set as Qcov=diag
10−8,10−8,10−8,10−8,0.8×10−12,0.8×10−12,0.8×10−12 (5.38) and
Rcov=diag
0.9781,0.9781,0.9781
. (5.39)
3. Prediction and correction: The prediction and correction stages follow the itera- tive procedure given in (5.5) to (5.10). The execution frequencies for both stages are 50 Hz. The measurement output vector yA,k at timekis the raw acceleration data delivered by the integrated accelerometer.
4. Signal update: In each loop, we need to update the following parameters: (i) the Euler anglesφ,θ, andψ, (ii) the corrected angular velocitiesωbb/n, (iii) the body frame proper acceleration at the CG, amea,b, (iv) the local NED acceleration at the CG, an, and (v) the local NED proper acceleration at the GPS antenna location, aant,n. We note that items (iii) to (v) are subjected to the lever arm effect (see Chap. 3). Also, item (v) will be used in the EKF for the INS. More specifically, we compute
φ θ ψ
=
⎛
⎜⎝
atan22(q2q3+q0q1)
1−2(q12+q22)
sin−1[−2(q1q3−q0q2)]
atan22(q1q2+q0q3)
1−2(q22+q32)
⎞
⎟⎠ (5.40)
and
ωbb/n=ωbb/n,mea−bω, (5.41)
whereωbb/n,meais the raw signal measurement of the gyroscope. We also compute
an=Rn/bamea,b+g, (5.42)
5.4 Design of Extended Kalman Filters 91 where
g= 0
0 g
(5.43) and
amea,b=amea− ˙ωbb/n×dCG−ωbb/n×(ωbb/n×dCG), (5.44) and where amea is the raw measurement of the accelerometer, dCG = [0 0 0.21]T is the position offset of the MNAV with respect to the CG in SheLion and HeLion in the body frame, and ω˙bb/nis the derivative of the cor- rected angular velocity that can be easily obtained based on the measurements at timekandk−1. Lastly, we calculate
aant,n=Rn/baant,b=Rn/b
amea,b+ ˙ωbb/n×doff+ωbb/n×(ωbb/n×doff) ,(5.45) where aant,bis the body frame proper acceleration at the GPS antenna location, and doff= [ −1.035 0 −0.172]T is the position offset of the GPS antenna with respect to the CG of HeLion and SheLion in the body frame. We note that in both (5.44) and (5.45), “×” denotes a cross product.
5.4.2 EKF for AHRS with Magnetometer Measurement
The AHRS dynamics model with the magnetometer measurement is characterized by (5.23) and (5.26). The EKF for the magnetometer-based AHRS is partially over- lapped with that of the accelerometer-based AHRS. In software realization, we may encounter a situation in which both filters are ready to execute in the same loop. In such a case, we set a lower priority for the EKF for the magnetometer-based AHRS.
1. Initial setting: The initial values for the state vector and error covariance matrix are identical to those given in (5.36) and (5.37), respectively.
2. Noise covariance matrices setting: Qcovis chosen to be identical to that in (5.38) and Rcovis set to
Rcov=0.0145. (5.46)
3. Prediction and correction: The prediction and correction stages, given in (5.5) to (5.10), for the EKF associated with the magnetometer-based AHRS are executed at different frequencies. More specifically, the prediction stage, shared with the EKF for the accelerometer-based AHRS, runs at a frequency of 50 Hz, whereas the correction stage is executed at a frequency of 10 Hz, which is the sampling frequency of the magnetometer. The measurement output vector yA,k at time k is the measurement of the heading angle provided by the magnetometer and is computed based on the measured magnetic field strength characterized by
ψmag=atan2
−Hy,n−Hy,off Hx,n−Hx,off
, (5.47)
92 5 Measurement Signal Enhancement whereHx,nandHy,nare the horizontal magnetic field measurement in the local NED frame, and Hx,off andHy,off are the magnetic field measurement offsets projected onto the local NED frame, which are mainly caused by some minor EMI of the unmanned system and can be determined by hard- and soft-iron cali- bration. They are obtained via the following coordinate transformation:
Hx,n=Hxcosθ+Hysinφsinθ+Hzcosφsinθ (5.48) and
Hy,n=Hycosφ−Hzsinφ, (5.49) whereHx,Hy, andHzare the raw magnetometer readings in the body frame. The Euler angles used in the above two equations are those updated in the previous time step, that is, timek−1.
4. Signal update: The signal update step is identical to its counterpart in the EKF associated with the accelerometer-based AHRS.
5.4.3 EKF for INS
The dynamics model for the INS is given earlier in (5.34) and (5.35). We summarize in what follows the parameters selected for the EKF associated with the INS.
1. Initial setting: Initial values for the state vector and the error covariance matrix are respectively given by
xI,0=[λref ϕref href 0 0 0 0 0 0 ]T (5.50) and
PI,0=I9. (5.51)
2. Noise covariance matrices setting: For the INS, Qcov and Rcov are respectively selected as
Qcov=diag
0,0,0,0.04,0.04,0.04,0.8×10−8,0.8×10−8,0.8×10−8 (5.52) and
Rcov=diag π
180 2
×10−10, π
180 2
×10−10,4,0.01,0.01,0.025
. (5.53) 3. Prediction and correction: The recursive procedure as in (5.5) to (5.10) is exe- cuted with a frequency of 4 Hz. The measurement output vector yI,kat timekis directly obtained from the GPS receiver.
4. Signal update: The EKF for the INS updates the position and velocity informa- tion in the local NED frame. We note that the position of the GPS antenna in the local NED frame can be determined by the coordinate transformations given