SE_2(3) based Extended Kalman Filtering and Smoothing Framework for Inertial-Integrated Navigation
Yarong Luo, Chi Guo, Shengyong You, Jianlang Hu, Jingnan Liu
aa r X i v : . [ c s . R O ] M a r S E (3) based Extended Kalman Filter and Smoothingfor Inertial-Integrated Navigation Yarong Luo, [email protected] Guo, [email protected] You, [email protected] Hu, [email protected] Liu, [email protected] Research Center, Wuhan University
Abstract
The error representation using the straight difference of two vectors in the inertial navigation sys-tem may not be reasonable as it does not take the direction difference into consideration. Therefore,we proposed to use the SE (3) matrix Lie group to represent the state of the inertial-integrated nav-igation system which consequently leads to the common frame error representation. With the newvelocity and position error definition, we leverage the group affine dynamics with the autonomouserror properties and derive the error state differential equation for the inertial-integrated navigationon the north-east-down local-level navigation frame and the earth-centered earth-fixed frame, re-spectively, the corresponding extending Kalman filter (EKF), terms as SE (3) -EKF has also beenderived. It provides a new perspective on the geometric EKF with a more sophisticated formulafor the inertial-integrated navigation system. Furthermore, we propose a SE (3) -based smoothingalgorithm based on the SE (3) -based EKF. Key Words SE (3) matrix Lie group, inertial-integrated navigation, common frame error representation, con-sistent extended Kalman filter, group-affine dynamics, autonomous error . Introduction The state error is commonly defined as the straight difference of the vectors without consideringthe vector’s frame representation [1]. This is unreasonable as the state error may not expressed withrespect to the same frame coordinate [2, 3], especially for the inertial-integrated navigation system.Li et al. proposed a common frame based unscented quaternion estimator which defines the attitudeerrors in the body frame expressed by the Uncented Kalman Filter and the IMU errors with respectto the common frame basis [3]. Wang et al. aimed at the coordinate-frame consistency of velocityerror vector, developed the state transformation extended Kalman filter (ST-EKF) for GPS/SINStightly couple integration [4], SINS/OD integration [5], and initial alignment of strapdown inertialnavigation system [6]. However, for the left invariant measurement model such as GNSS, ST-EKF used the right invariant error definition which is not consistent with left-innovation update.Recently, Chang studied the SE (3) based EKF for spacecraft attitude estimation which formulasthe attitude and gyroscope bias as elements of SE(3) [7].The above works motivates us to embed the state and the uncertainties into a specially definedand high dimensional matrix Lie group SE (3) so that the state evolves on matrix manifold. Weconsider the state error on the SE (3) matrix Lie group by defining the error as the product be-tween the true state and the inverse of the estimated state so that not only the velocity error isexpressed with respect to the common frame, but also the position error is expressed with respectto the common frame. The significant advantage of this common frame based representation is thatthe resulting expressions are compact and accurate by means of the Lie group theory. Althoughthis work is motivated by Geometric EKF [8] for quaternion based inertial navigation system, thewhole derivations is completely different from it as we parameterize the attitude by direction co-sine matrix (DCM). As the whole state is confined to matrix Lie group, it avoids the over-parameterand normalization constraint of the quaternion representation. Meanwhile, we extended the ST-EKF [4] by considering the position error with respect to the common frame. Then, we investigatethe state error dynamic equations from the local geodetic latitude-longitude-height frame perspec-tive and the local-level north-east-down frame perspective. Furthermore, we give the invariantmeasurement model involving lever arm error and show the equivalence of between the invariantmeasurement model and the traditional EKF’s measurement model. Based on the left-invariant2 E (3) -based EKF, we propose the SE (3) -based smoothing algorithm. Finally, we derive theleft-invariant SE (3) -EKF and the right-invariant SE (3) -EKF and compare their performance onthe inertial-integrated navigation.This remainder of this paper is organized as follows. Preliminaries are presented in Section 2.In section 3 the SE (3) based EKF for NED navigation with navigation frame attitude error is de-rived. Section 4 formulates the SE (3) based EKF for NED navigation with estimated navigationframe attitude error. The SE (3) based EKF for NED navigation with body frame attitude error isgiven in Section 5. Conclusion and future work are given in Section 6.
2. Preliminaries
The kinematics of the vehicles are described by the velocity, position and the direction, which areexpressed on the manifold space and identified by different frames. The velocity and position canbe represented by the vectors and the attitude in the 3-dimensional vector space can be representedby the direction cosine matrix (DCM). This three quantities can be reformulated as an element ofthe SE (3) matrix Lie group. Meanwhile, the vector v cab describes the vector points from point a topoint b and expressed in the c frame. The direction cosine matrix C fd represents the rotation fromthe d frame to the f frame. Therefore, we summarize the commonly used frames in the inertialnavigation and give detailed navigation equations in both the NED frame and the ECEF frame. SE (3) Matrix Lie Group
The SE (3) matrix Lie group is also called the group of direct spatial isometries [9] and it repre-sents the space of matrices that apply a rigid body rotation and 2 translations to points in R . More-over, the group SE (3) has the structure of the semidirect product of SO(3) group by R × R andcan be expressed as SE (3) = SO (3) ⋉ R × R | {z } [10]. The relationship between the Lie algebraand the associated vector is described by a linear isomorphism Λ : R → se (3) , i.e. Λ( ξ ) = φ × ϑ ζ × × ∈ se (3) , ∀ ξ = φϑζ ∈ R , φ, ϑ, ζ ∈ R (1)3he exponential mapping from the Lie algebra to the corresponding Lie group is given as T = exp G (Λ( ξ )) = ∞ X n =0 n ! (Λ( ξ )) n = exp G φ × ϑ ζ × × = exp G ( φ × ) J ϑ J ζ × × (2)where φ × denotes the skew-symmetric matrix generated from a 3D vector φ ∈ R ; exp G denotesthe matrix exponential mapping; J is the left Jacobian matrix of the 3D orthogonal rotation matricesgroup SO (3) which is given by. J = J l ( φ ) = ∞ X n =0 n + 1)! ( φ ∧ ) n = I + 1 − cos θθ φ ∧ + θ − sin θθ φ ∧ , θ = || φ || (3)The closed form expression for T from the exponential map can also be obtained as T = ∞ X n =0 n ! (Λ( ξ )) n = I × + Λ( ξ ) + 1 − cos θθ Λ( ξ ) + θ − sin θθ Λ( ξ ) (4) SE (3) is common used as the extended poses (orientation, velocity, position) for 3-dimensionalinertial navigation. SE (3) The uncertainties on matrix Lie group SE (3) can be represented by left multiplication and rightmultiplicationleft multiplication : T l = ˆ T exp G (Λ( ε l )) = ˆ T exp G (Λ( ε l )) ˆ T − ˆ T = exp G (Λ( Ad ˆ T ( ε l ))) ˆ T right multiplication : T r = exp G (Λ( ε r )) ˆ T (5)Therefore, the probability distributions for the random variables T ∈ SE (3) can be definedas left-invariant concentrated Gaussian distribution on SE (3) and right-invariant concentratedGaussian distribution on SE (3) :left-invariant : T ∼ N L ( ˆ T , P ) , T l = ˆ T exp G (Λ( ε l )) , ε l ∼ N (0 , P ) right-invariant : T ∼ N R ( ˆ T , P ) , T r = exp G (Λ( ε r )) ˆ T , ε r ∼ N (0 , P ) (6)where N ( · , · ) is the classical Gaussian distribution in Euclidean space and P ∈ R K +1) × K +1) is a covariance matrix. The invariant property can be verified by exp G (Λ( ε r )) = ( T r Γ)( ˆ T Γ) − = T r ˆ T − and exp G (Λ( ε l )) = (Γ ˆ T ) − (Γ T l ) = ˆ T − T l . The noise-free quantity ˆ T is viewed as themean, and the dispersion arises through left multiplication or right multiplication with the matrixexponential of a zero mean Gaussian random variable.4 .3. Reference Frames The commonly used reference frames in inertial-integrated navigation system are summarized inthis section.Earth-Centered-Inertial (ECI) Frames (i-frame) is an ideal frame of reference in which idealaccelerometers and gyroscopes fixed to the i-frame have zero outputs and it has its origin at thecenter of the Earth and axes that are non-rotating with respect to the fixed stars with its z-axisparallel to the spin axis of the Earth, x-axis pointing towards the mean vernal equinox, and y-axiscompleting a right-handed orthogonal frame.Earth-Centered-Earth-Fixed (ECEF) Frames (e-frame) has its origin at the center of mass ofthe Earth and axes that are fixed with respect to the Earth. Its x-axis points towards the meanmeridian of Greenwich, z-axis is parallel to the mean spin axis of the Earth, and y-axis completesa right-handed orthogonal frame.Navigation Frames (n-frame) is a local geodetic frame which has its origin coinciding withthat of the sensor frame, with its x-axis pointing towards geodetic north, z-axis orthogonal to thereference ellipsoid pointing down, and y-axis completing a right-handed orthogonal frame, i.e. thenorth-east-down (NED) system. The local geodetic coordinate system can be represented by northcoordinate X, east coordinate Y and height Z (XYZ, units:m, m, m), or by latitude ϕ , longitude λ and height h (LLH, unit: rad, rad, m), and longitude and latitude can be converted one-to-one toXY.Body Frames (b-frame) is an orthogonal axis set which is fixed onto the vehicle and rotate withit, therefore, it is aligned with the roll, pitch and heading axes of a vehicle, i.e. forward-transversal-down. The attitude in the NED frame can be represented by the DCM C nb , The differential equation of C nb and C bn are given by ˙ C nb = C nb ( ω bib × ) − ( ω nin × ) C nb (7) ˙ C bn = C bn ( ω nin × ) − ( ω bib × ) C bn (8)5here ω bib is the angular rate vector of the body frame relative to the inertial frame expressed inthe body frame; ω nin is the angular rate vector of the navigation frame relative to the inertial frameexpressed in the navigation frame.The differential equation of the velocity vector in the NED local-level navigation frame is givenby ˙ v neb = C nb f bib − [(2 ω nie + ω nen ) × ] v neb + g n (9)where ω nie is the earth rotation vector expressed in the navigation frame; f bib is the specific forcevector in navigation frame; ω nen = ω nin − ω nie is the angular rate vector of the navigation framerelative to the earth frame expressed in the navigation frame which is also call the transport rate;and g n is the gravity vector.The differential equation of the velocity error with all parameters expressed in the navigationframe can be written as follows: δ ˙ v neb = ( C nb f bib ) × φ n + C nb δf bib − (2 ω nie + ω nen ) × δv neb − (2 δω nie + δω nen ) × v neb + δg n (10)where δv neb and φ n are the velocity error vector and attitude error vector, respectively; δω nie and δω nen are the angular rate errors corresponding to ω nie and ω nen , respectively; δg n is the normalgravity error in the local navigation frame. ω nie and ω nen can be given as follows ω nie = ω ie cos ϕ − ω ie sin ϕ , ω nen = ˙ λ cos ϕ − ˙ ϕ − ˙ λ sin ϕ = v E R N + h − v N R M + h − v E tan ϕR N + h ω nin = ω nie + ω nen = ω ie cos ϕ + v E R N + h − v N R M + h − ω ie sin ϕ − v E tan ϕR N + h , ω nie + ω nen = ω ie cos ϕ + v E R N + h − v N R M + h − ω ie sin ϕ − v E tan ϕR N + h (11)where ω ie = 0 . rad/s is the magnitude of the earth’s rotation angular rate; v N and v E are velocities in the north and east direction, respectively; h is ellipsoidal height; R M and R N are radii of curvature in the meridian and prime vertical; ˙ ϕ = v N R M + h and ˙ λ = v E ( R N + h ) cos ϕ areused in the derivation. 6hen the position vector is expressed in terms of the geodetic latitude ϕ , longitude λ , andheight h , the differential equation of the position vector is given by ˙ r leb = ˙ ϕ ˙ λ ˙ h = R M + h R N + h ) cos ϕ
00 0 − v N v E v D = N rv v neb (12)The corresponding position error differential equation can be written as [11] δ ˙ ϕ = − v N ( R M + h ) δh + 1 R M + h δv N δ ˙ λ = v E tan ϕ ( R N + h ) cos ϕ δϕ − v E ( R N + h ) cos ϕ δh + 1( R N + h ) cos ϕ δv E δ ˙ h = − δv D (13)where δϕ , δλ , and δh are the latitude error, the longitude error, and the height error respectively.Therefore, the matrix form of the position error differential equation in terms of the geodeticlatitude, longitude and elevation is δ ˙ ϕδ ˙ λδ ˙ h = − v N ( R M + h ) v E tan ϕ ( R N + h ) cos ϕ − v E ( R N + h ) cos ϕ δϕδλδh + R M + h R N + h ) cos ϕ
00 0 − δv N δv E δv D = δ ˙ r leb = N rr δr leb + N rv δv neb (14)Perturbations on ω nie , ω nen , and ω nin can be given as follows δω nie = − ω ie sin ϕδr N R M + h − ω ie cos ϕδr N R M + h = − ω ie sin ϕδϕ − ω ie cos ϕδϕ = − ω ie sin ϕ − ω ie cos ϕ δϕδλδh = N δr leb (15)where δϕ = δr N R M + h is used. δω nen = v E δr D ( R N + h ) + δv E R N + h − v N δr D ( R M + h ) − δv N R M + h − v E δr N ( R N + h )( R M + h ) cos ϕ − v E tan ϕδr D ( R N + h ) − tan ϕδv E R N + h = − v E ( R N + h ) v N ( R M + h ) − v E ( R N + h ) cos ϕ v E tan ϕ ( R N + h ) δr leb + R N + h − R M + h − tan ϕR N + h δv neb = N δr leb + N δv neb (16)7here δh = − δr D is used. δω nin = δω nie + δω nen = N δr leb + N δr leb + N δv neb = ( N + N ) δr leb + N δv neb (17) The position error state expressed in radians is usually very small, which will cause numericalinstability in Kalman filter calculation. Therefore, it is usually to represent the position errorvector in terms of the XYZ coordinate system, that is δr neb = δr N δr E δr D = ( R M + h ) δϕ ( R N + h ) cos ϕδλ − δh (18)The differential equation of the position vector in the XYZ local-level navigation frame is givenby [12] δ ˙ r N = ( ˙ R M + ˙ h ) δϕ + ( R M + h ) δ ˙ ϕδ ˙ r E = ( R N + h ) cos ϕδ ˙ λ − ( R N + h ) sin ϕδλ ˙ ϕ + ( ˙ R N + ˙ h ) cos ϕδλδ ˙ r D = − δ ˙ h (19)Substituting position differential equation (12) and position error differential equation (13) intoabove equation, we can get the matrix form of the the position error differential equation in termsof the NED coordinate system δ ˙ r N δ ˙ r E δ ˙ r D = − v D ( R M + h ) v N ( R M + h ) v E tan ϕ ( R N + h ) − tan ϕv N ( R M + h ) − v D ( R N + h ) v E ( R N + h ) δr N δr E δr D + δv N δv E δv D = δ ˙ r neb = − ω nen δr neb + δθ × v neb + δv neb = F rr δr neb + δv neb (20)where δθ is the difference between the computer frame and the true navigation frame [13] and canbe calculated by δθ = δr E R N + h − δr N R M + h − δr E tan ϕR N + h (21)8he position vector differential equation in terms of the NED coordinate system can be calcu-lated as ˙ r neb = ddt ( C ne r eeb ) = ddt ( C ne ) r eeb + C ne ˙ r eeb = C ne ( ω ene × ) r eeb + C ne v eeb = − ω nen × r neb + v neb (22)Perturbations on ω nie , ω nen , and ω nin can be given as follows δω nie = − ω ie sin ϕδr N R M + h − ω ie cos ϕδr N R M + h = M δr neb (23) δω nen = v E δr D ( R N + h ) + δv E R N + h − v N δr D ( R M + h ) − δv N R M + h − v E δr N ( R N + h )( R M + h ) cos ϕ − v E tan ϕδr D ( R N + h ) − tan ϕδv E R N + h = M δr neb + M δv neb (24) δω nin = δω nie + δω nen = M δr neb + M δr neb + M δv neb = ( M + M ) δr neb + M δv neb (25) The sensor errors of the accelerometers and gyroscopes are modeled as one-order Gauss-Markovmodel: δf bib = b a + w a , ˙ b a = − τ a b a + w b a (26) δω bib = b g + w g , ˙ b g = − τ g b g + w b g (27)where w a and w g are the Gaussian white noises of the accelerometers and gyroscopes, respec-tively; w b a and w b g are the Gaussian white noises of the accelerometer biases and gyroscope bi-ases, respectively; τ a and τ g are the correlation times of accelerometer biases and gyroscope biases,respectively. SE (3) based EKF for NED Navigation with navigation Frame Attitude Error The velocity vector v neb , position vector r neb and attitude matrix C nb can formula the element of the SE (3) matrix Lie group 9 = C nb v neb r neb × × ∈ SE (3) (28)The inverse of the element can be written as follows X − = C bn − C bn v neb − C bn r neb × × = C bn − v beb − r beb × × ∈ SE (3) (29)Therefore, the differential equation of the X can be calculated as ddt X = f u t ( X ) = ddt C nb v neb r neb × × = ˙ C nb ˙ v neb ˙ r neb × × = X W + W X = C nb ( ω bib × ) − ( ω nin × ) C nb C nb f bib − [(2 ω nie + ω nen ) × ] v neb + g n − ω nen × r neb + v neb × × (30)where u t is a sequence of inputs; W and W are denoted as W = ω bib × f bib × × , W = − ω nin × g n − ω nie × v neb v neb + ω nie × r neb × × (31)It is easy to verify that the dynamical equation f u t ( X ) is group-affine and the group-affinesystem owns the log-linear property of the corresponding error propagation [9]: f u t ( X A ) X B + X A f u t ( X B ) − X A f u t ( I d ) X B =( X A W + W X A ) X B + X A ( X B W + W X B ) − X A ( W + W ) X B = X A X B W + W X A X B , f u t ( X A X B ) (32)Similar to the state error defined in the Euclidean space, i.e. the difference of the truth minusthe estimate, we can define the state error on the matrix Lie group as the group operation that thetrue state multiplies the inverse of the estimated state, i.e. η = X ˜ X − , this is where all the errorsare defined in the navigation frame. It is obvious that the error defined on the matrix Lie group is10ight invariant by the right action of the matrix Lie group and it is verified by η = ( X R )( ˜ X R ) − = X ˜ X − , ∀ R ∈ SE (3) . Therefor we can define the state error on the matrix Lie group by attitudeerror component, the velocity error component and the position error component as follows η , X ˜ X − = C nb ˜ C bn v neb − C nb ˜ C bn ˜ v neb r neb − C nb ˜ C bn ˜ r neb × × , η a η v η r × × ∈ SE (3) (33)where η a is the attitude error expressed in the navigation frame; η v is the velocity error expressedin the navigation frame; η r is the position error expressed in the navigation frame.According to the matrix exponential mapping from the Lie algebra to the matrix Lie group, thestate error can be converted back to the corresponding Lie algebra as follows η , exp G ( φ n × ) J ρ nv J ρ nr × × = exp G φ n × ρ nv ρ nr × × = exp G Λ φ n ρ nv ρ nr = exp G (Λ( ρ )) (34)where φ n is the attitude error expressed in the navigation frame; Λ( · ) represents a linear isomor-phism between the vector space R and the Lie algebra se (3) ; exp G represents the matrix expo-nential mapping from the Lie algebra to the Lie group; ρ = (cid:16) ( φ n ) T ( ρ nv ) T ( ρ nr ) T (cid:17) T representsthe Lie algebra corresponding to the state error η ; exp G ( φ n × ) is the Rodriguez formula of therotation vector; J is the left Jacobian matrix of the Rodriguez formula and can be calculated by exp G ( φ n × ) = cos φI × + 1 − cos φφ φ n φ nT + sin φφ ( φ n × ) , φ = || φ n || (35) J = sin φφ I × + 1 φ (1 − sin φφ ) φ n φ nT + 1 − cos φφ ( φ n × ) , φ = || φ n || (36)Comparing equation (33) and equation (34), we can get η a = exp G ( φ n × ) = C nb ˜ C bn ≈ I × + φ n × , if || φ n || is small (37) η v = J ρ nv = v neb − C nb ˜ C bn ˜ v neb ≈ v neb − ( I × + φ n × )˜ v neb = δv neb − φ n × ˜ v neb = δv neb + ˜ v neb × φ n (38) η r = J ρ nr = r neb − C nb ˜ C bn ˜ r neb ≈ r neb − ( I × + φ n × )˜ r neb = δr neb − φ n × ˜ r neb = δr neb + ˜ r neb × φ n (39)It is obvious from the above equations we can find the new state error definition is differentfrom the traditional state error definition. The new state error takes into account the magnitude11nd direction difference of the state vectors in both the true navigation frame and the calculatednavigation frame. This is a new perspective of the common frame error definition by a moresophisticated formula that using the difference of two elements form the SE (3) matrix Lie group.This is more nature for the navigation system modeling as the state truly evolves on the SE (3) matrix Lie group and reasonable for the error definition for all inertial-integrated navigation asorientation variations may lead inconsistent error [3]. Meanwhile, it is also worth noting that thisnew error definition extends the ST-EKF [4] by redefining the position error.Now, we consider the differential equations for the attitude error, velocity error, and positionerror which can form a element of the SE (3) matrix Lie group. On the one hand, by takingdifferential of attitude error η r with respect to time, we can get ddt η a = ddt C nb ˜ C bn = ˙ C nb ˜ C bn + C nb ˙˜ C bn = (cid:0) C nb ( ω bib × ) − ( ω nin × ) C nb (cid:1) ˜ C bn + C nb (cid:16) ˜ C bn (˜ ω nin × ) − (˜ ω bib × ) ˜ C bn (cid:17) = C nb ( ω bib × ) ˜ C bn − ( ω nin × ) C nb ˜ C bn + C nb ˜ C bn (˜ ω nin × ) − C nb (˜ ω bib × ) ˜ C bn ≈ − ( ω nin × )( I × + φ n × ) + ( I × + φ n × )(( ω nin + δω nin ) × ) − C nb (cid:0) ˜ ω bib − ω bib ) × (cid:1) ˜ C bn ≈ δω nin × +( φ n × ω nin ) × +( φ n × )( δω nin × ) − δω nib × ( I + φ n × ) ≈ δω nin × +( φ n × ω nin ) × − δω nib × (40)where the 2-order small quantities ( φ n × )( δω nin × ) and ( δω nib × )( φ n × ) are neglected at the last step; δω nin is defined as δω nin , ˜ ω nin − ω nin ; δω bib is defined as δω bib , ˜ ω bib − ω bib .On the other hand, ddt η a ≈ ddt ( I × + φ n × ) = ˙ φ n × (41)Therefore, the state error differential equation for the attitude error can be written as follows ˙ φ n = δω nin + ( φ n × ω nin ) − δω nib = − ω nin × φ n + δω nin − C nb δω bib (42)By taking differential of velocity error η v with respect to time and substituting equation (9) into12t, we can get ddt η v = ddt (cid:16) v neb − C nb ˜ C bn ˜ v neb (cid:17) = ˙ v neb − ddt ( C nb ˜ C bn )˜ v neb − C nb ˜ C bn ˙˜ v neb = C nb f bib − [(2 ω nie + ω nen ) × ] v neb + g n − C nb ˜ C bn h ˜ C nb ˜ f bib − [(2˜ ω nie + ˜ ω nen ) × ] ˜ v neb + ˜ g n i − h C nb ( ω bib × ) ˜ C bn − ( ω nin × ) C nb ˜ C bn + C nb ˜ C bn (˜ ω nin × ) − C nb (˜ ω bib × ) ˜ C bn i ˜ v neb = − C nb ( ˜ f bib − f bib ) − [(2 ω nie + ω nen ) × ] ( C nb ˜ C bn ˜ v neb + η v ) + C nb ˜ C bn [(2˜ ω nie + ˜ ω nen ) × ] ˜ v neb + ( ω nin × ) C nb ˜ C bn ˜ v neb − C nb ˜ C bn (˜ ω nin × )˜ v neb − C nb ( ω bib × ) ˜ C bn ˜ v neb + C nb (˜ ω bib × ) ˜ C bn ˜ v neb + ( g n − C nb ˜ C bn ˜ g n ) ≈ − C nb δf bib − ( ω nie × ) C nb ˜ C nb ˜ v neb − [(2 ω nie + ω nen ) × ] η v + C nb ˜ C bn (˜ ω nie × )˜ v neb − C nb ( ω bib − ˜ ω bib ) C bn C nb ˜ C bn ˜ v neb + ( g n − ˜ g n ) − φ n × ˜ g n ≈ − C nb δf bib − [(2 ω nie + ω nen ) × ] η v + δω nie × ˜ v neb − ( ω nie × )( φ n × )˜ v neb + ( φ n × )( ω nie × )˜ v neb + φ n × δω nie × ˜ v neb + ( C nb δω bib ) × ˜ v neb + ( C nb δω bib ) × φ n × ˜ v neb − δg n − φ n × ˜ g n = − C nb δf bib − [(2 ω nie + ω nen ) × ] η v + δω nie × ˜ v neb − ( ω nie × φ n ) × ˜ v neb + φ n × δω nie × ˜ v neb + ( C nb δω bib ) × ˜ v neb + ( C nb δω bib ) × φ n × ˜ v neb − δg n − φ n × ˜ g n ≈ − C nb δf bib − [(2 ω nie + ω nen ) × ] η v − ˜ v neb × δω nie + (˜ v neb × )( ω nie × ) φ n − ˜ v neb × ( C nb δω bib ) − δg n + ˜ g n × φ n (43)where 2-order small quantities ( C nb δω bib ) × φ n × ˜ v neb and φ n × δω nie × ˜ v neb are neglected at the last step; δf bib is defined as δf bib , ˜ f bib − f bib ; δω nie is defined as δω nie , ˜ ω nie − ω nie ; δg n is defined as δg n , ˜ g n − g n and it can be neglected as the change of g n is quite small for carrier’s local navigation.As we can see that there is no specific force term f bib in the relationship between the attitudeerror term and the velocity error term. The result and merits have been shown in ST-EKF [4].By taking differential of position error η r with respect to time and substituting equation (22)13nto it, we can get ddt η r = ddt ( r neb − C nb ˜ C bn ˜ r neb ) = ˙ r neb − ddt ( C nb ˜ C bn )˜ r neb − C nb ˜ C bn ˙˜ r neb =( − ω nen × r neb + v neb ) − C nb ˜ C bn ( − ˜ ω nen × ˜ r neb + ˜ v neb ) − h C nb ( ω bib × ) ˜ C bn − ( ω nin × ) C nb ˜ C bn + C nb ˜ C bn (˜ ω nin × ) − C nb (˜ ω bib × ) ˜ C bn i ˜ r neb = − ( ω nen × )( C nb ˜ C bn ˜ r neb + η r ) + ( v neb − C nb ˜ C bn ˜ v neb ) + C nb ˜ C bn (˜ ω nen × )˜ r neb + ( ω nin × ) C nb ˜ C bn ˜ r neb − C nb ˜ C bn (˜ ω nin × )˜ r neb − C nb ( ω bib × ) ˜ C bn ˜ r neb + C nb (˜ ω bib × ) ˜ C bn ˜ r neb = − ω nen × η r + η v + ( ω nie × ) C nb ˜ C bn ˜ r neb − C nb ˜ C bn (˜ ω nie × )˜ r neb + C nb ( δω bib × ) ˜ C bn ˜ r neb ≈ − ω nen × η r + η v + ( ω nie × )( φ n × )˜ r neb − ( φ n × )( ω nie × )˜ r neb − δω nie × ˜ r neb − φ n × δω nie × ˜ r neb + δω nib × ˜ r neb + δω nib × φ n × ˜ r neb = − ω nen × η r + η v + ( ω nie × φ n ) × ˜ r neb − δω nie × ˜ r neb − φ n × δω nie × ˜ r neb + δω nib × ˜ r neb + δω nib × φ n × ˜ r neb ≈ − ω nen × η r + η v − (˜ r neb × )( ω nie × ) φ n + ˜ r neb × δω nie − ˜ r neb × ( C nb δω bib ) (44)where 2-order small quantities ( C nb δω bib ) × φ n × ˜ r neb and φ n × δω nie × ˜ r neb are neglected at the laststep.With the new definition of the attitude error, velocity error, and position error, we substituteequation (38) and equation (39) into equation (24) and equation (25): δω nie = M δr neb = M ( η r − ˜ r neb × φ n ) (45) δω nin = ( M + M ) δr neb + M δv neb = ( M + M )( η r − ˜ r neb × φ n ) + M ( η v − ˜ v neb × φ n ) (46)Consequently, the state error dynamical equations with respect to the navigation frame can bewritten as follows ˙ φ n = − ω nin × φ n + ( M + M )( η r − ˜ r neb × φ n ) + M ( η v − ˜ v neb × φ n ) − C nb ( b g + w g )=( M + M ) η r + M η v − (( ω nin × ) + M (˜ v neb × ) + ( M + M )(˜ r neb × )) φ n − C nb ( b g + w g ) (47) ddt η v = − C nb ( b a + w a ) − [(2 ω nie + ω nen ) × ] η v − (˜ v neb × ) M ( η r − ˜ r neb × φ n )+ (˜ v neb × )( ω nie × ) φ n − ˜ v neb × ( C nb ( b g + w g )) + ˜ g n × φ n = − (˜ v neb × ) M η r − [(2 ω nie + ω nen ) × ] η v − ˜ v neb × ( C nb ( b g + w g ))+ ((˜ v neb × ) M (˜ r neb × ) + (˜ v neb × )( ω nie × ) + (˜ g n × )) φ n − C nb ( b a + w a ) (48)14 dt η r = − ω nen × η r + η v − (˜ r neb × )( ω nie × ) φ n + ˜ r neb × M ( η r − ˜ r neb × φ n ) − ˜ r neb × ( C nb ( b g + w g ))=(˜ r neb × M − ( ω nen × )) η r + η v − ((˜ r neb × )( ω nie × ) + (˜ r neb × ) M (˜ r neb × )) φ n − ˜ r neb × ( C nb ( b g + w g )) (49)If the position is represented in terms of LLH, δω nie and δω nin can be calculated as δω nie = N δr leb = N ( η r − ˜ r leb × φ n ) (50) δω nin = ( N + N ) δr leb + N δv neb = ( N + N )( η r − ˜ r leb × φ n ) + N ( η v − ˜ v neb × φ n ) (51)Then, the new differential equations of attitude error, velocity error and position error can becalculated as ˙ φ n = − ω nin × φ n + ( N + N )( η r − ˜ r leb × φ n ) + N ( η v − ˜ v neb × φ n ) − C nb ( b g + w g )=( N + N ) η r + N η v − (cid:0) ( ω nin × ) + N (˜ v neb × ) + ( N + N )(˜ r leb × ) (cid:1) φ n − C nb ( b g + w g ) (52) ddt η v = − C nb ( b a + w a ) − [(2 ω nie + ω nen ) × ] η v − (˜ v neb × ) N ( η r − ˜ r leb × φ n )+ (˜ v neb × )( ω nie × ) φ n − ˜ v neb × ( C nb ( b g + w g )) + ˜ g n × φ n = − (˜ v neb × ) N η r − [(2 ω nie + ω nen ) × ] η v − ˜ v neb × ( C nb ( b g + w g ))+ (cid:0) (˜ v neb × ) N (˜ r leb × ) + (˜ v neb × )( ω nie × ) + (˜ g n × ) (cid:1) φ n − C nb ( b a + w a ) (53) ddt η r = ddt ( r leb − C nb ˜ C bn ˜ r leb ) ≈ ddt ( r leb − ˜ r leb − φ n × ˜ r leb ) = ddt ( δr leb + ˜ r leb × φ n )=( N rr δr leb + N rv δv neb ) + (( ˜ N rv ˜ v neb ) × ) φ n + (˜ r leb × )( − ω nin × φ n + δω nin − C nb δω bib )= (cid:0) N rr ( η r − ˜ r leb × φ n ) + N rv ( η v − ˜ v neb × φ n ) (cid:1) + (cid:16) ( ˜ N rv ˜ v neb ) × (cid:17) φ n + (˜ r leb × )( N + N ) η r + (˜ r leb × ) N η v − (˜ r leb × ) (cid:0) ( ω nin × ) + N (˜ v neb × ) + ( N + N )(˜ r leb × ) (cid:1) φ n − (˜ r leb × ) C nb ( b g + w g )= (cid:0) N rr + (˜ r leb × )( N + N ) (cid:1) η r + (cid:0) N rv + (˜ r leb × ) N (cid:1) η v − (cid:16) N rr (˜ r leb × ) + N rv (˜ v neb × ) − (cid:16) ( ˜ N rv ˜ v neb ) × (cid:17) + (˜ r leb × )( ω nin × ) + (˜ r leb × ) N (˜ v neb × )+(˜ r leb × )( N + N )(˜ r leb × ) (cid:1) φ n − (˜ r leb × ) C nb ( b g + w g ) (54)15 . SE (3) based EKF for NED Navigation with estimated navigation frame Attitude Error If we define the state error as the group operator that the estimate multiplies the truth, then ε =( ˜ X R )( X R ) − = ˜ X X − , ∀ R ∈ SE (3) . This is where all the errors are defined in the estimatednavigation frame. ε , ˜ X X − = ˜ C nb C bn ˜ v neb − ˜ C nb C bn v neb ˜ r neb − ˜ C nb C bn r neb × × , ε a ε v ε r × × ∈ SE (3) (55)where ε a is the attitude error expressed in the estimated navigation frame; ε v is the velocityerror expressed in the estimated navigation frame; ε r is the position error expressed in the estimatednavigation frame.According to the matrix exponential mapping from the Lie algebra to the matrix Lie group, thestate error can be converted back to the corresponding Lie algebra as follows ε , exp G ( φ ˜ n × ) J ρ ˜ nv J ρ ˜ nr × × = exp G φ ˜ n × ρ ˜ nv ρ ˜ nr × × = exp G Λ φ ˜ n ρ ˜ nv ρ ˜ nr = exp G (Λ( ˜ ρ )) (56)where φ ˜ n is the attitude error expressed in the estimated navigation frame; ˜ ρ = (cid:16) ( φ ˜ n ) T ( ρ ˜ nv ) T ( ρ ˜ nr ) T (cid:17) T represents the Lie algebra corresponding to the state error ε .Comparing equation (55) with equation (56), we can get ε a = exp G ( φ ˜ n × ) = ˜ C nb C bn ≈ I × + φ ˜ n × , if || φ ˜ n || is small (57) ε v = J ρ ˜ nv = ˜ v neb − ˜ C nb C bn v neb ≈ ˜ v neb − ( I × + φ ˜ n × ) v neb = δv neb − φ ˜ n × v neb = δv neb + v neb × φ ˜ n (58) ε r = J ρ ˜ nr = ˜ r neb − ˜ C nb C bn r neb ≈ ˜ r neb − ( I × + φ ˜ n × ) r neb = δr neb − φ ˜ n × r neb = δr neb + r neb × φ ˜ n (59)Now, we consider the differential equations for the attitude error, velocity error, and positionerror which can form a element of the SE (3) matrix Lie group. On the one hand, by taking16ifferential of attitude error ε r with respect to time, we can get ddt ε a = ddt ˜ C nb C bn = ˙˜ C nb C bn + ˜ C nb ˙ C bn = (cid:16) ˜ C nb (˜ ω bib × ) − (˜ ω nin × ) ˜ C nb (cid:17) C bn + ˜ C nb (cid:0) C bn ( ω nin × ) − ( ω bib × ) C bn (cid:1) = ˜ C nb (˜ ω bib × ) C bn − (˜ ω nin × ) ˜ C nb C bn + ˜ C nb C bn ( ω nin × ) − ˜ C nb ( ω bib × ) C bn ≈ ˜ C nb ((˜ ω bib − ω bib ) × ) C bn − (( ω nin + δω nin ) × )( I × + φ n × ) + ( I × + φ n × )( ω nin × ) ≈ ( I + φ n × )( δω nib × ) − ( δω nin × ) + ( φ n × ω nin ) × − ( δω nin × )( φ n × ) ≈ − δω nin × +( φ n × ω nin ) × + δω nib × (60)where the 2-order small quantities ( δω nin × )( φ n × ) and ( φ n × )( δω nib × ) are neglected at the last step; δω nin is defined as δω nin , ˜ ω nin − ω nin ; δω bib is defined as δω bib , ˜ ω bib − ω bib .On the other hand, ddt η a ≈ ddt ( I × + φ n × ) = ˙ φ n × (61)Therefore, the state error differential equation for the attitude error can be written as follows ˙ φ n = − δω nin + ( φ n × ω nin ) + δω nib = − ω nin × φ n − δω nin + C nb δω bib (62)By taking differential of velocity error η v with respect to time and substituting equation (58)17nto it, we can get ddt ε v = ddt (cid:16) ˜ v neb − ˜ C nb C bn v neb (cid:17) = ˙˜ v neb − ddt ( ˜ C nb C bn ) v neb − ˜ C nb C bn ˙ v neb = h ˜ C nb ˜ f bib − [(2˜ ω nie + ˜ ω nen ) × ] ˜ v neb + ˜ g n i − ˜ C nb C bn (cid:2) C nb f bib − [(2 ω nie + ω nen ) × ] v neb + g n (cid:3) − h ˜ C nb (˜ ω bib × ) C bn − (˜ ω nin × ) ˜ C nb C bn + ˜ C nb C bn ( ω nin × ) − ˜ C nb ( ω bib × ) C bn i v neb = ˜ C nb ( ˜ f bib − f bib ) − [(2˜ ω nie + ˜ ω nen ) × ] ( ε v + ˜ C nb C bn v neb ) + ˜ C nb C bn [(2 ω nie + ω nen ) × ] v neb − ˜ C nb (˜ ω bib × ) C bn v neb + (˜ ω nin × ) ˜ C nb C bn v neb − ˜ C nb C bn ( ω nin × ) v neb + ˜ C nb ( ω bib × ) C bn v neb + (˜ g n − ˜ C nb C bn g n ) ≈ ˜ C nb δf bib − (˜ ω nie × ) ˜ C nb C nb v neb − [(2˜ ω nie + ˜ ω nen ) × ] ε v + ˜ C nb C bn ( ω nie × ) v neb − ˜ C nb C bn C nb (˜ ω bib − ω bib ) C bn v neb + (˜ g n − g n ) − φ n × g n ≈ C nb δf bib − [(2˜ ω nie + ˜ ω nen ) × ] ε v − δω nie × v neb − ( ω nie × )( φ n × ) v neb + ( φ n × )( ω nie × ) v neb − δω nie × φ n × v neb − ( C nb δω bib ) × v neb − φ n × ( C nb δω bib ) × v neb + δg n + g n × φ n = ˜ C nb δf bib − [(2˜ ω nie + ˜ ω nen ) × ] ε v − δω nie × v neb − ( ω nie × φ n ) × v neb − δω nie × φ n × v neb − ( C nb δω bib ) × v neb − φ n × ( C nb δω bib ) × v neb + δg n + g n × φ n ≈ ˜ C nb δf bib − [(2˜ ω nie + ˜ ω nen ) × ] ε v + v neb × δω nie + ( v neb × )( ω nie × ) φ n + ( v neb × ) C nb δω bib + δg n + g n × φ n (63)where 2-order small quantities φ n × ( C nb δω bib ) × v neb and ω nie × φ n × δ ˜ v neb are neglected at the last step; δf bib is defined as δf bib , ˜ f bib − f bib ; δω nie is defined as δω nie , ˜ ω nie − ω nie ; δg n is defined as δg n , ˜ g n − g n and it can be neglected as the change of g n is quite small for carrier’s local navigation.By taking differential of position error η r with respect to time and substituting equation (59)18nto it, we can get ddt ε r = ddt (˜ r neb − ˜ C nb C bn r neb ) = ˙˜ r neb − ddt ( ˜ C nb C bn ) r neb − ˜ C nb C bn ˙ r neb =( − ˜ ω nen × ˜ r neb + ˜ v neb ) − ˜ C nb C bn ( − ω nen × r neb + v neb ) − h ˜ C nb (˜ ω bib × ) C bn − (˜ ω nin × ) ˜ C nb C bn + ˜ C nb C bn ( ω nin × ) − ˜ C nb ( ω bib × ) C bn i r neb = − (˜ ω nen × )( ˜ C nb C bn r neb + ε r ) + (˜ v neb − ˜ C nb C bn v neb ) + ˜ C nb C bn ( ω nen × ) r neb + (˜ ω nin × ) ˜ C nb C bn r neb − ˜ C nb C bn ( ω nin × ) r neb − ˜ C nb (˜ ω bib × ) C bn r neb + ˜ C nb ( ω bib × ) C bn r neb = − ˜ ω nen × ε r + ε v + (˜ ω nie × ) ˜ C nb C bn r neb − ˜ C nb C bn ( ω nie × ) r neb − ˜ C nb ( δω bib × ) C bn r neb ≈ − ˜ ω nen × ε r + ε v + ( ω nie × )( φ n × ) r neb − ( φ n × )( ω nie × ) r neb + δω nie × r neb + δω nie × φ n × r neb − δω nib × r neb − φ n × δω nib × r neb = − ˜ ω nen × ε r + ε v + ( ω nie × φ n ) × r neb + δω nie × r neb + δω nie × φ n × r neb − δω nib × r neb − φ n × δω nib × r neb ≈ − ˜ ω nen × ε r + ε v − ( r neb × )( ω nie × ) φ n − r neb × δω nie + r neb × ( C nb δω bib ) (64)where 2-order small quantities φ n × ( C nb δω bib ) × r neb and δω nie × φ n × r neb are neglected at the laststep.Similar to equation (45) and equation (46), we can get δω nie = M δr neb = M ( ε r − r neb × φ n ) (65) δω nin = ( M + M ) δr neb + M δv neb = ( M + M )( ε r − r neb × φ n ) + M ( ε v − v neb × φ n ) (66)Consequently, the state error dynamical equations with respect to the estimated navigationframe can be written as follows ˙ φ n = − ω nin × φ n − ( M + M )( ε r − r neb × φ n ) − M ( ε v − v neb × φ n ) + C nb ( b g + w g )= − ( M + M ) ε r − M ε v − (( ω nin × ) − M ( v neb × ) − ( M + M )( r neb × )) φ n + C nb ( b g + w g ) (67) ddt ε v = C nb ( b a + w a ) − [(2˜ ω nie + ˜ ω nen ) × ] ε v + ( v neb × ) M ( η r − r neb × φ n )+ ( v neb × )( ω nie × ) φ n + v neb × ( C nb ( b g + w g )) + g n × φ n =( v neb × ) M ε r − [(2˜ ω nie + ˜ ω nen ) × ] ε v + v neb × ( C nb ( b g + w g ))+ ( − ( v neb × ) M ( r neb × ) + ( v neb × )( ω nie × ) + ( g n × )) φ n + C nb ( b a + w a ) (68)19 dt ε r = − ω nen × ε r + ε v − ( r neb × )( ω nie × ) φ n − r neb × M ( ε r − r neb × φ n ) + r neb × ( C nb ( b g + w g ))= − ( r neb × M + ( ω nen × )) ε r + ε v − (( r neb × )( ω nie × ) − ( r neb × ) M ( r neb × )) φ n + r neb × ( C nb ( b g + w g )) (69)If the position is represented in terms of LLH, δω nie and δω nin can be calculated as δω nie = N δr leb = N ( η r − r leb × φ n ) (70) δω nin = ( N + N ) δr leb + N δv neb = ( N + N )( η r − r leb × φ n ) + N ( η v − v neb × φ n ) (71)Then, the new differential equations of attitude error, velocity error and position error can becalculated as ˙ φ n = − ω nin × φ n − ( N + N )( η r − ˜ r leb × φ n ) − N ( η v − ˜ v neb × φ n ) + C nb ( b g + w g )= − ( N + N ) η r − N η v − (cid:0) ( ω nin × ) − N ( v neb × ) − ( N + N )( r leb × ) (cid:1) φ n + C nb ( b g + w g ) (72) ddt ε v = C nb ( b a + w a ) − [(2˜ ω nie + ˜ ω nen ) × ] ε v + ( v neb × ) N ( ε r − r leb × φ n )+ ( v neb × )( ω nie × ) φ n + ( v neb × )( C nb ( b g + w g )) + g n × φ n =( v neb × ) N ε r − [(2˜ ω nie + ˜ ω nen ) × ] ε v + v neb × ( C nb ( b g + w g ))+ (cid:0) − ( v neb × ) N ( r leb × ) + ( v neb × )( ω nie × ) + ( g n × ) (cid:1) φ n + C nb ( b a + w a ) (73) ddt ε r = ddt (˜ r leb − ˜ C nb C bn r leb ) ≈ ddt (˜ r leb − r leb − φ n × r leb ) = ddt ( δr leb + r leb × φ n )=( N rr δr leb + N rv δv neb ) + (( N rv v neb ) × ) φ n + ( r leb × )( − ω nin × φ n − δω nin + C nb δω bib )= (cid:0) N rr ( ε r − r leb × φ n ) + N rv ( ε v − v neb × φ n ) (cid:1) + (( N rv v neb ) × ) φ n − ( r leb × )( N + N ) ε r − ( r leb × ) N ε v − ( r leb × ) (cid:0) ( ω nin × ) + N (˜ v neb × ) + ( N + N )(˜ r leb × ) (cid:1) φ n + ( r leb × ) C nb ( b g + w g )= (cid:0) N rr − ( r leb × )( N + N ) (cid:1) ε r + (cid:0) N rv − ( r leb × ) N (cid:1) ε v − (cid:0) N rr ( r leb × ) + N rv ( v neb × ) − (( N rv v neb ) × ) + ( r leb × )( ω nin × ) + ( r leb × ) N ( v neb × )+( r leb × )( N + N )( r leb × ) (cid:1) φ n + ( r leb × ) C nb ( b g + w g ) (74)20 . SE (3) based EKF for NED Navigation with Body Frame Attitude Error When the error state is left invariant by the left group action, this is the world-centric estimatorformulation and is suitable for sensors such as GNSS, 5G, etc. If the error state is converted to thetrue body frame, i.e., ε l = ( L X ) − ( L ˜ X ) = X − ˜ X ∈ SE (3) , then ε l = X − ˜ X = C bn ˜ C nb C bn (˜ v neb − v neb ) C bn (˜ r neb − r neb )0 × × = ε a ε v ε r × × (75)The error state can be converted to the corresponding Lie algebra as follows: ε l , exp G ( φ b × ) J ρ bv J ρ br × × = exp G φ b × ρ bv ρ br × × = exp G Λ φ b ρ bv ρ br = exp G (cid:0) Λ( ρ b ) (cid:1) (76)where φ b is the attitude error expressed in the body frame; ρ b = (cid:16) ( φ b ) T ( ρ bv ) T ( ρ br ) T (cid:17) T repre-sents the Lie algebra corresponding to the state error ε l ;Comparing equation (75) and equation (76), we can get ε a = exp G ( φ b × ) = C bn ˜ C nb ≈ I × + φ b × , if || φ b || is small (77) ε v = J ρ bv = C bn (˜ v neb − v neb ) = C bn δv neb (78) ε r = J ρ br = C bn (˜ r neb − r neb ) = C bn δr neb (79)Now, we consider the differential equations for the attitude error, velocity error, and positionerror which can form a element of the SE (3) matrix Lie group. On the one hand, by taking21ifferential of attitude error ε a with respect to time, we can get ddt ε a = ddt C bn ˜ C nb = ˙ C bn ˜ C nb + C bn ˙˜ C nb = (cid:0) C bn ( ω nin × ) − ( ω bib × ) C bn (cid:1) ˜ C nb + C bn (cid:16) ˜ C nb (˜ ω bib × ) − (˜ ω nin × ) ˜ C nb (cid:17) = C bn ( ω nin × ) ˜ C nb − ( ω bib × ) C bn ˜ C nb + C bn ˜ C nb (˜ ω bib × ) − C bn (˜ ω nin × ) ˜ C nb ≈ − ( ω bib × )( I × + φ b × ) + ( I × + φ b × )(˜ ω bib × ) − C bn ((˜ ω nin − ω nin ) × ) ˜ C nb = − ω bib × − ( ω bib × )( φ b × ) + ˜ ω bib × +( φ b × )(˜ ω bib × ) − C bn ( δω nin × ) C nb C bn ˜ C nb ≈ δω bib × +( φ b × ω bib ) × +( φ b × )( δω bib × ) − δω bin × − ( δω bin × )( φ b × ) ≈ δω bib × +( φ b × ω bib ) × − δω bin × (80)where the 2-order small quantities ( φ b × )( δω bib × ) and ( δω bin × )( φ b × ) are neglected at the last step; δω nin is defined as δω nin , ˜ ω nin − ω nin ; δω bib is defined as δω bib , ˜ ω bib − ω bib .On the other hand, ddt ε a ≈ ddt ( I × + φ b × ) = ˙ φ b × (81)Therefore, the state error differential equation for the attitude error can be written as follows ˙ φ b = δω bib + ( φ b × ω bib ) − δω bin = − ω bib × φ b + δω bib − C bn δω nin (82)By taking differential of velocity error ε v with respect to time and substituting equation (78)into it, we can get ddt ε v = ddt (cid:0) C bn (˜ v neb − v neb ) (cid:1) = ˙ C bn (˜ v neb − v neb ) + C bn ( ˙˜ v neb − ˙ v neb )= (cid:0) C bn ( ω nin × ) − ( ω bib × ) C bn (cid:1) δv neb + C bn nh ˜ C nb ˜ f bib − [(2˜ ω nie + ˜ ω nen ) × ] ˜ v neb + ˜ g n i − (cid:2) C nb f bib − [(2 ω nie + ω nen ) × ] v neb + g n (cid:3)o = C bn ( ω nin × ) δv neb − ( ω bib × ) C bn δv neb + C bn ˜ C nb ˜ f bib − C bn [(2˜ ω nie + ˜ ω nen ) × ] ˜ v neb + C bn ˜ g n − C bn C nb f bib + C bn [(2 ω nie + ω nen ) × ] v neb − C bn g n ≈ C bn ( ω nin × ) δv neb − ( ω bib × ) C bn δv neb + δf bib + φ b × ˜ f bib + C bn δg n − C bn (2 ω nie + ω nen ) × δv neb − C bn (2 δω nie + δω nen ) × ˜ v neb = − ( ω bib × ) ε v + δf bib + φ b × ˜ f bib + C bn δg n − C bn ω nie × δv neb − C bn ( δω nie + δω nin ) × ˜ v neb = − ( ω bib × ) ε v + δf bib + φ b × ˜ f bib + C bn δg n − ( C bn ω nie ) × ε v + C bn ˜ v neb × ( δω nie + δω nin ) (83)22here δf bib is defined as δf bib , ˜ f bib − f bib ; δω nie is defined as δω nie , ˜ ω nie − ω nie ; δg n is definedas δg n , ˜ g n − g n and it can be neglected as the change of g n is quite small for carrier’s localnavigation.By taking differential of position error ε r with respect to time and substituting equation (79)into it, we can get ddt ε r = ddt (cid:0) C bn (˜ r neb − r neb ) (cid:1) = ˙ C bn (˜ r neb − r neb ) + C bn ( ˙˜ r neb − ˙ r neb )= (cid:0) C bn ( ω nin × ) − ( ω bib × ) C bn (cid:1) δr neb + C bn [( − ˜ ω nen × ˜ r neb + ˜ v neb ) − ( − ω nen × r neb + v neb )]= C bn ( ω nin × ) δr neb − ( ω bib × ) C bn δr neb + C bn δv neb − C bn (˜ ω nen × ˜ r neb − ω nen × r neb )= C bn ( ω nin × ) δr neb − ( ω bib × ) C bn δr neb + C bn δv neb − C bn ( δω nen × r neb + ˜ ω nen × δr neb )= − ( ω bib × ) ε r + ε v + C bn r neb × δω nen + C bn ω nie × δr neb − C bn ( δω nen × δr neb ) ≈ − ( ω bib × ) ε r + ε v + C bn r neb × δω nen + ( C bn ω nie ) × ε r (84)where 2-order small quantities C bn ( δω nen × δr neb ) is neglected at the last step.With the new definition of the attitude error, velocity error, and position error, we substituteequation (78) and equation (79) into equation (24) and equation (25): δω nie = M δr neb = M C nb ε r (85) δω nin = ( M + M ) δr neb + M δv neb = ( M + M ) C nb ε r + M C nb ε v (86)Consequently, the state error dynamical equations with respect to the true body frame can bewritten as follows: ˙ φ b = − ω bib × φ b + ( b g + w g ) − C bn (( M + M ) C nb ε r + M C nb ε v )= − C bn ( M + M ) C nb ε r − C bn M C nb ε v − ω bib × φ b + ( b g + w g ) (87) ddt ε v = − ( ω bib × ) ε v + δf bib + φ b × ˜ f bib + C bn δg n − ( C bn ω nie ) × ε v + C bn ˜ v neb × ( M C nb ε r + ( M + M ) C nb ε r + M C nb ε v )= C bn (˜ v neb × )(2 M + M ) C nb ε r + ( C bn (˜ v neb × ) M C nb − ( ω bib × ) − ( C bn ω nie ) × ) ε v − ˜ f bib × φ b + C bn δg n + ( b a + w a ) (88) ddt ε r = − ( ω bib × ) ε r + ε v + C bn r neb × ( M C nb ε r + M C nb ε v ) + ( C bn ω nie ) × ε r = (cid:0) C bn ( r neb × ) M C nb − ( ω bib × ) + ( C bn ω nie ) × (cid:1) ε r + (cid:0) I + C bn ( r neb × ) M C nb (cid:1) ε v (89)23 . SE (3) based EKF for NED Navigation with estimated Body Frame Attitude Error If the state error is converted to the estimated body frame, i.e., ε l = ( L ˜ X ) − ( L X ) = ˜ X − X ∈ SE (3) , the error is left invariant. The definition of the error on matrix Lie group as inverse ofthe estimated state multiplies the true state is similar to the error defined on the Euclidean space astrue vector minus estimated vector.The error state is given as ε e = ˜ X − X = ˜ C bn C nb ˜ C bn ( v neb − ˜ v neb ) ˜ C bn ( r neb − ˜ r neb )0 × × = ε a ε v ε r × × (90)The error state can be converted to the corresponding Lie algebra as follows: ε e , exp G ( φ ˜ b × ) J ρ ˜ bv J ρ ˜ br × × = exp G φ ˜ b × ρ ˜ bv ρ ˜ br × × = exp G Λ φ ˜ b ρ ˜ bv ρ ˜ br = exp G (cid:16) Λ( ρ ˜ b ) (cid:17) (91)where φ ˜ b is the attitude error expressed in the estimated body frame; ρ ˜ b = (cid:16) ( φ ˜ b ) T ( ρ ˜ bv ) T ( ρ ˜ br ) T (cid:17) T represents the Lie algebra corresponding to the state error ε e ;Comparing equation (90) and equation (91), we can get ε a = exp G ( φ ˜ b × ) = ˜ C bn C nb ≈ I × + φ ˜ b × , if || φ ˜ b || is small (92) ε v = J ρ ˜ bv = ˜ C bn ( v neb − ˜ v neb ) = − ˜ C bn δv neb (93) ε r = J ρ ˜ br = ˜ C bn ( r neb − ˜ r neb ) = − ˜ C bn δr neb (94)Now, we consider the differential equations for the attitude error, velocity error, and positionerror which can form a element of the SE (3) matrix Lie group. On the one hand, by taking24ifferential of attitude error ε a with respect to time, we can get ddt ε a = ddt ˜ C bn C nb = ˙˜ C bn C nb + ˜ C bn ˙ C nb = (cid:16) ˜ C bn (˜ ω nin × ) − (˜ ω bib × ) ˜ C bn (cid:17) C nb + ˜ C bn (cid:0) C nb ( ω bib × ) − ( ω nin × ) C nb (cid:1) = ˜ C bn (˜ ω nin × ) C nb − (˜ ω bib × ) ˜ C bn C nb + ˜ C bn C nb ( ω bib × ) − ˜ C bn ( ω nin × ) C nb ≈ − (˜ ω bib × )( I × + φ b × ) + ( I × + φ b × )( ω bib × ) + ˜ C bn ((˜ ω nin − ω nin ) × ) C nb = − ˜ ω bib × − (˜ ω bib × )( φ b × ) + ω bib × +( φ b × )( ω bib × ) + ˜ C bn C nb C bn ( δω nin × ) C nb ≈ − δω bib × +( φ b × ω bib ) × − ( δω bib × )( φ b × ) + δω bin × + φ b × ( δω bin × ) ≈ − δω bib × +( φ b × ω bib ) × + δω bin × (95)where the 2-order small quantities ( δω bib × )( φ b × ) and ( φ b × )( δω bin × ) are neglected at the last step; δω nin is defined as δω nin , ˜ ω nin − ω nin ; δω bib is defined as δω bib , ˜ ω bib − ω bib .On the other hand, ddt ε a ≈ ddt ( I × + φ ˜ b × ) = ˙ φ ˜ b × (96)Therefore, the state error differential equation for the attitude error can be written as follows ˙ φ ˜ b = − δω bib + ( φ ˜ b × ω bib ) + δω bin = − ω bib × φ ˜ b − δω bib + C bn δω nin (97)By taking differential of velocity error ε v with respect to time and substituting equation (93)into it, we can get ddt ε v = ddt (cid:16) ˜ C bn ( v neb − ˜ v neb ) (cid:17) = ˙˜ C bn ( v neb − ˜ v neb ) + ˜ C bn ( ˙ v neb − ˙˜ v neb )= − (cid:16) ˜ C bn (˜ ω nin × ) − (˜ ω bib × ) ˜ C bn (cid:17) δv neb + ˜ C bn n(cid:2) C nb f bib − [(2 ω nie + ω nen ) × ] v neb + g n (cid:3) − h ˜ C nb ˜ f bib − [(2˜ ω nie + ˜ ω nen ) × ] ˜ v neb + ˜ g n io = − ˜ C bn (˜ ω nin × ) δv neb + (˜ ω bib × ) ˜ C bn δv neb + ˜ C bn C nb f bib − ˜ C bn [(2 ω nie + ω nen ) × ] v neb + ˜ C bn g n − ˜ C bn ˜ C nb ˜ f bib + ˜ C bn [(2˜ ω nie + ˜ ω nen ) × ] ˜ v neb − ˜ C bn ˜ g n ≈ − ˜ C bn (˜ ω nin × ) δv neb + (˜ ω bib × ) ˜ C bn δv neb − δf bib + φ b × f bib − ˜ C bn δg n + ˜ C bn (2˜ ω nie + ˜ ω nen ) × δv neb + ˜ C bn (2 δω nie + δω nen ) × v neb = − (˜ ω bib × ) ε v − δf bib + φ b × f bib − ˜ C bn δg n + ˜ C bn (˜ ω nie × ) ˜ C nb ˜ C bn δv neb + ˜ C bn ( δω nie + δω nin ) × v neb = − (˜ ω bib × ) ε v − δf bib + φ b × f bib − ˜ C bn δg n − ( ˜ C bn ˜ ω nie ) × ε v − ˜ C bn ( v neb × )( δω nie + δω nin ) ≈ − ( ω bib × ) ε v − δf bib + φ b × f bib − C bn δg n − ( C bn ω nie ) × ε v − C bn ( v neb × )( δω nie + δω nin ) (98)25here δf bib is defined as δf bib , ˜ f bib − f bib ; δω nie is defined as δω nie , ˜ ω nie − ω nie ; δg n is definedas δg n , ˜ g n − g n and it can be neglected as the change of g n is quite small for carrier’s localnavigation.By taking differential of position error ε r with respect to time and substituting equation (94)into it, we can get ddt ε r = ddt (cid:16) ˜ C bn ( r neb − ˜ r neb ) (cid:17) = ˙˜ C bn ( r neb − ˜ r neb ) + ˜ C bn ( ˙ r neb − ˙˜ r neb )= − (cid:16) ˜ C bn (˜ ω nin × ) − (˜ ω bib × ) ˜ C bn (cid:17) δr neb + ˜ C bn [( − ω nen × r neb + v neb ) − ( − ˜ ω nen × ˜ r neb + ˜ v neb )]= − ˜ C bn (˜ ω nin × ) δr neb + (˜ ω bib × ) ˜ C bn δr neb − ˜ C bn δv neb + ˜ C bn (˜ ω nen × ˜ r neb − ω nen × r neb )= − ˜ C bn (˜ ω nin × ) δr neb + (˜ ω bib × ) ˜ C bn δr neb − ˜ C bn δv neb + ˜ C bn ( δω nen × ˜ r neb + ω nen × δr neb )= − (˜ ω bib × ) ε r + ε v − ˜ C bn (˜ r neb × ) δω nen − ˜ C bn ω nie × ˜ C nb ˜ C bn δr neb − ˜ C bn ( δω nin × ) δr neb ≈ − (˜ ω bib × ) ε r + ε v − ˜ C bn (˜ r neb × ) δω nen + ( ˜ C bn ω nie ) × ε r ≈ − ( ω bib × ) ε r + ε v − C bn ( r neb × ) δω nen + ( C bn ω nie ) × ε r (99)where 2-order small quantities ˜ C bn ( δω nin × δr neb ) is neglected at the last step.With the new definition of the attitude error, velocity error, and position error, we substituteequation (93) and equation (94) into equation (24) and equation (25): δω nie = M δr neb = − M ˜ C nb ε r (100) δω nin = ( M + M ) δr neb + M δv neb = − ( M + M ) ˜ C nb ε r − M ˜ C nb ε v (101)Consequently, the state error dynamical equations with respect to the true body frame can bewritten as follows: ˙ φ b = − ω bib × φ b − ( b g + w g ) + C bn ( − ( M + M ) ˜ C nb ε r − M ˜ C nb ε v )= − C bn ( M + M ) ˜ C nb ε r − C bn M ˜ C nb ε v − ω bib × φ b − ( b g + w g ) (102) ddt ε v = − ( ω bib × ) ε v − δf bib + φ b × f bib − C bn δg n − ( C bn ω nie ) × ε v + C bn v neb × ( M C nb ε r + ( M + M ) C nb ε r + M C nb ε v )= C bn ( v neb × )(2 M + M ) C nb ε r + ( C bn ( v neb × ) M C nb − ( ω bib × ) − ( C bn ω nie ) × ) ε v − f bib × φ b − C bn δg n − ( b a + w a ) (103)26 dt ε r = − ( ω bib × ) ε r + ε v + C bn r neb × ( M C nb ε r + M C nb ε v ) + ( C bn ω nie ) × ε r = (cid:0) C bn ( r neb × ) M C nb − ( ω bib × ) + ( C bn ω nie ) × (cid:1) ε r + (cid:0) I + C bn ( r neb × ) M C nb (cid:1) ε v (104)It is obvious that the only difference between the true body frame and the estimate body framelies in the δf bib term and the δω bib term. The GNSS provides navigation information in a global frame and has the left-invariant measure-ment equations on matrix Lie group. GNSS positioning solution gives the position coordinates ofthe antenna phase center(or other reference point), while GINS’s mechanization gives the naviga-tion results of the IMU measurement center. The two do not coincide physically, so the integratednavigation needs to correct the lever arm effect. In the case of the arm lever error, we rearrangeevery measurement from GNSS as: y t = r nGNSS = C nb v neb r neb × × l b + r t , X t b + V t (105)where r nGNSS is the positioning result calculated by GNSS and expressed in the navigation frame; l b is the lever arm measurement vector expressed in the body frame; r t is measurement white noisewith covariance R t .Then, the left-innovation can be defined as z t = ˜ X − t y t − b = ˜ X − t ( X t b + V t ) − b = ε e b − b + ˜ X − t V t ≈ ( I + Λ( ρ ˜ b )) b − b + ˜ X − t V t = Λ( ρ ˜ b ) b + ˜ X − t V t = φ ˜ b × ρ ˜ bv ρ ˜ br × × l b + ˜ C bn − ˜ v beb − ˜ r beb × × r t = φ ˜ b × l b + ρ ˜ br + ˜ C bn r t = Hρ ˜ b + ˜ V t (106)where H can be abbreviated as its reduced form as H rt = h − l b × × I × i by considering thecomputational efficiency, and H is independent of the system state, but only related to the known27ector b . ˜ V t can be abbreviated as ˜ r t = ˜ C bn r t = ( ˜ C nb ) − r t = M t r t . It is worth noting that theinvariant-innovation can be termed as innovation expressed in the body frame. Remark 1
From the definition of left-innovation, the inverse of the estimated system state is usedto multiply the measurement is reasonable as the we get the state-independent measurement matrixH. Meanwhile, the form of the left-innovation can be viewed as analogous to the GNSS positioningresults minus the GINS predicted values which is .
When the biased of the acceleration and gyroscope are considered, the innovation vector canbe quantified as ˜ z t = h − l b × × I × × × i ρ ˜ b ξ b + ˜ V t , H t δx + M t r t (107)where ξ b represents error state about the bias term.Therefore, the Kalman filter gain can be partitioned into two parts: K t = K ζt K ξt = P t H Tt ( H t P t H Tt + M t R t M Tt ) − (108)The covariance update can be calculated as P + t = ( I − K t H t ) P t ( I − K t H t ) T + K t M t R t M Tt K Tt (109) SE (3) based EKF measurement equation In the SE (3) based EKF, the measurement vector can be represented as the difference of theposition expressed in the n frame calculated by GNSS and the position expressed in the n framecalculated by GINS: δz l = ˜ r nGNSS − ˜ r nGINS = r nGNSS + r t − (˜ r nimu + ˜ C nb l b )= r nGNSS + r t − (cid:16) r nimu + δr nimu + C nb ( I − φ ˜ b × ) l b (cid:17) = r nGNSS − (cid:0) r nimu + C nb l b (cid:1) − δr nimu + C nb ( φ ˜ b × ) l b + r t = − δr nimu + C nb ( φ ˜ b × ) l b + r t = − δr neb − C nb ( l b × ) φ ˜ b + r t = ˜ C nb J ρ ˜ br − C nb ( l b × ) φ ˜ b + r t ≈ ˜ C nb ρ ˜ br − ˜ C nb ( l b × ) φ ˜ b + r t , H new δx + r t (110)28here H new is the SE (3) based measurement matrix and is defined as H new = h − ˜ C nb ( l b × ) 0 × ˜ C nb × × i (111)It is worth noting that the innovation is expressed in the navigation frame which is different fromthe invariant-innovation defined in equation(106).Comparing equation(107) and equation(111) we can find H new = ˜ C nb H t ⇒ H t = ˜ C bn H new (112)Then, by considering the Kalman filter gain in the SE (3) -based EKF, the Kalman gain in the SE (3) based EKF can be written as K new = P t H Tnew (cid:0) H new P t H Tnew + R t (cid:1) − = P t H Tnew ˜ C nb (cid:16) ˜ C bn H new P t H Tnew ˜ C nb + ˜ C bn R t ˜ C nb (cid:17) − ˜ C bn = P t (cid:16) ˜ C bn H new (cid:17) T (cid:18)(cid:16) ˜ C bn H new (cid:17) P t (cid:16) ˜ C bn H new (cid:17) T + M t R t M Tt (cid:19) − ˜ C bn = P t H Tt ( H t P t H Tt + M t R t M Tt ) − ˜ C bn = K t ˜ C bn (113)As all the KF algorithms execute the reset state in closed loop after each measurement updatestep, the error state will be set as ”zero” to indicate the nominal value is the same as the estima-tion [11]. Consequently, there is no need to implement the error state prediction step after feedbackis made, and the correction of the error state can be described as ˆ x ≈ K z ˜ z t + x = K t ˜ z t (114)Substituting equation(107) and equation(108) into the above equation, we can get ˆ x = K t ˜ z t = P t H Tt ( H t P t H Tt + M t R t M Tt ) − ( H t δx + M t r t )= K new ˜ C nb ( ˜ C bn H new δx + M t r t ) = K new ˜ C nb ( ˜ C bn H new δx + ˜ C bn r t )= K new ( H new δx + r t ) = K new δz l (115) Remark 2
It is obvious that the correction of the error state by the invariant-EKF and the SE (3) based EKF is the same. Therefore, the invariant-EKF can be viewed as the correction step of errorstate in the body frame but the SE (3) based EKF executes the correction of error state in thenavigation frame. emark 3 Our SE (3) based EKF can be used in H ∞ filtering similar to the invariant extended H ∞ filter [14].
7. EKF and SE (3) based EKF for ECEF Navigation When the system state is defined as X = C eb v eib r eib × × ∈ SE (3) (116)where C eb is the direction cosine matrix from the body frame to the ECEF frame; v eib is the velocityof body frame relative to the ECI frame expressed in the ECEF frame; r eib is the position of bodyframe relative to the ECI frame expressed in the ECEF frame.As the ECEF frame has the same origin as the ECI frame, so r iie = 0 and r iib = r iie + r ieb = r ieb = C ie r eeb . Meanwhile, we also get r eib = r eeb = C ei r ieb .The differential equation of the attitude C eb is given as ˙ C eb = C eb ( ω bib × ) − ( ω eie × ) C eb (117)As the velocity has the relationship v eib = C ei v iib , so the differential equation of the velocity v eib can be calculated as ˙ v eib = ddt ( C ei v iib ) = ˙ C ei v iib + C ei ˙ v iib = ( − ω eie × ) C ei v iib + C ei (cid:0) C ib f b + G iib (cid:1) = ( − ω eie × ) v eib + C ei C ib f b + C ei G iib = ( − ω eie × ) v eib + C eb f b + G eib (118)where G eib is the gravity acceleration expressed in the ECEF frame.The differential equation of the position r eib is given as v eeb = ˙ r eeb = ˙ r eib = ( − ω eie × ) C ei r iib + C ei ˙ r iib = ( − ω eie × ) r eib + v eib (119)According to the differential equation of position (119) we can know that v eib = v eeb + ( ω eie × ) r eib ,so the differential equation of velocity v eib can also be deduced as follows: ˙ v eib = ˙ v eeb + ( ω eie × ) ˙ r eib (120)30he earth gravity equation is given as g eib = C ei g iib = C ei (cid:0) G iib − ( ω iie × ) r ieb (cid:1) = C ei G iib − C ei ( ω iie × ) C ie C ei ( ω iie × ) C ie C ei r ieb = G eib − ( ω eie × ) r eeb (121)Substituting equation(121) into equation(120) and we can get ˙ v eib = C eb f b + g eib − ω eie × ) v eeb + ( ω eie × ) ˙ r eeb = C eb f b + g eib − ( ω eie × ) v eeb = C eb f b + g eib − ( ω eie × )(( − ω eie × ) r eib + v eib )= C eb f b + g eib + ( ω eie × ) r eib − ( ω eie ) × v eib = C eb f b + G eib − ( ω eie ) × v eib (122)This result is the same as the equation(118).In the end, we get the differential equations of the attitude, velocity and the position in theECEF frame. When the state is defined as in equation(116), the invariant property of the dynamiccan also be proofed in a similar approach. However, they just used the right invariant error stateprediction for the inertial-integrated navigation which is not consistent with the right invariant-EKFfrom the perspective of invariance as the GNSS measurements are left-invariant.Therefore, we give the left-invariant EKF in detail for the first time, which is also the frameworkof the SE (3) based EKF. Firstly, the state and its inverse defined on the matrix Lie group is givenas X = C eb v eib r eib , X − = C be − v bib − r bib (123)31hen the dynamic equation of the state X can be deduced as follows ddt X = f u t ( X ) = ddt C eb v eib r eib = ˙ C eb ˙ v eib ˙ r eib = C eb ( ω bib × ) − ( ω eie × ) C eb ( − ω eie × ) v eib + C eb f b + G eib ( − ω eie × ) r eib + v eib = C eb v eib r eib ω bib × f b × + − ω eie × G eib v eib C eb v eib r eib = X W + W X (124)It is easy to verified the dynamical equation(124) satisfies the group-affine property similar toequation(32). SE (3) based EKF for ECEF Navigation with Body Frame Attitude Error Considering the measurements of the GNSS are left-invariant observations for the world-centricobserver, we first give the left-invariant error state differential equations. The left-invariant error isdefined as η L = X − ˆ X = C be − v bib − r bib ˆ C eb ˆ v eib ˆ r eib = C be ˆ C eb C be ˆ v eib − v bib C be ˆ r eib − r bib (125)According to the map form the Lie algebra to the Lie group, the error states of attitude, velocity,and position can de derived as C be ˆ C eb = exp G ( φ b × ) ≈ I + φ b × η Lv = J ρ bv = C be ˆ v eib − v bib = C be ˆ v eib − C be v eib = C be (ˆ v eib − v eib ) = C be δv eib η Lr = J ρ br = C be ˆ r eib − r bib = C be ˆ r eib − C be r eib = C be (ˆ r eib − r eib ) = C be δr eib (126)32eanwhile, the left-invariant error satisfies that η L = exp G ( φ b × ) J ρ bv J ρ br = exp G ( φ b ) × ρ bv ρ br = exp G Λ φ b ρ bv ρ br (127)where φ b is the attitude error state, J ρ bv is the new definition of velocity error state, J ρ br is the newdefinition of position error state; J is the left Jacobian matrix given in equation(36).The differential equation of the attitude error state is given as ddt ( C be ˆ C eb ) = ˙ C be ˆ C eb + C be ˙ˆ C eb = (cid:2) C be ( ω eie × ) − ( ω bib × ) C be (cid:3) ˆ C eb + C be h ˆ C eb (ˆ ω bib × ) − (ˆ ω eie × ) ˆ C eb i = C be ( ω eie × ) ˆ C eb − ( ω bib × ) C be ˆ C eb + C be ˆ C eb (ˆ ω bib × ) − C be ( ω eie × ) ˆ C eb ≈ − ( ω bib × )( I + φ b × ) + ( I + φ b × )(( ω bib + δω bib ) × )= − ( ω bib × )( φ b × ) + ( δω bib ) × +( φ b × )( ω bib × ) + φ b × ( δω bib ) ×≈ ( φ b × ω bib ) × + δω bib × = ( φ b × ω bib ) × +( δb bg + w bg ) × (128)where the angular velocity error of the earth’s rotation can be neglected, i.e., ˆ ω eie = ω eie ; andsecond order small quantity ( φ b × )( δω bib × ) is also neglected. Therefore, the equation(128) can besimplified as ˙ φ b = φ b × ω bib + δω bib = − ω bib × φ b + δω bib = − ω bib × φ b + δb bg + w bg (129)The differential equation of the velocity error state is given as ddt ( J ρ bv ) = ˙ C be δv eib + C be ( ˙ˆ v eib − ˙ v eib )= (cid:2) C be ( ω eie × ) − ( ω bib × ) C be (cid:3) δv eib + C be (cid:16)h ( − ˆ ω eie × )ˆ v eib + ˆ C eb ˆ f b + ˆ G eib i − (cid:2) ( − ω eie × ) v eib + C eb f b + G eib (cid:3)(cid:17) = C be ( ω eie × ) δv eib − ( ω bib × ) C be δv eib + C be ˆ C eb ˆ f b − C be C eb f b − C be ω eie × (ˆ v eib − v eib )+ C be ( ˆ G eib − G eib ) ≈ − ( ω bib × ) J ρ bv + ( I + φ b × )( f b + δb ba + w ba ) − f b + C be ( ˆ G eib − G eib )= − ( ω bib × ) J ρ bv + φ b × f b + φ b × δf b + C be ( ˆ G eib − G eib ) + δf b ≈ − ( ω bib × ) J ρ bv − f b × φ b + C be ( ˆ G eib − G eib ) + δf b = − ( ω bib × ) J ρ bv − f b × φ b + C be ( ˆ G eib − G eib ) + δb ba + w ba (130)33here the second order small quantity φ b × δf b is neglected; and as G eib can be approximated asconstant, C be ( ˆ G eib − G eib ) can also be neglected.In the same way, the differential equation of the position error state is given as ddt ( J ρ br ) = ˙ C be δr eib + C be ( ˙ˆ r eib − ˙ r eib )= (cid:2) C be ( ω eie × ) − ( ω bib × ) C be (cid:3) δr eib + C be ([( − ˆ ω eie × )ˆ r eib + ˆ v eib ] − [( − ω eie × ) r eib + v eib ])= C be ( ω eie × ) δr eib − ( ω bib × ) C be δr eib − C be ω eie × (ˆ r eib − r eib ) + C be (ˆ v eib − v eib )= − ω bib × J ρ br + J ρ bv (131)Thus, the inertial-integrated error state dynamic equation for the SE (3) based EKF can beobtained δ ˙ x = F δx + Gw (132)where F is the error state transition matrix; δx is the error state including the terms about bias; Gis the noise driven matrix. Their definition is given as x = φ b J ρ bv J ρ br δb bg δb ba , F = − ω bib × I − f b × − ω bib × I I − ω bib × − τ g
00 0 0 0 − τ a , G = I I , w = w bg w ba w bb g w bb a (133) SE (3) based EKF measurement equation If the lever arm error is taken into account, the measurement error vector is expressed in the ECEFframe as the difference between the position calculated by INS and the position calculated by34NSS: δz r = ˆ r eGINS − ˆ r eGNSS = ˆ r eIMU + ˆ C eb l b − r eGNSS + n GNSS ≈ r eIMU + δr eIMU + C eb ( I + φ b × ) l b − r eGNSS + n GNSS = r eIMU + C eb l b − r eGNSS + δr eIMU + C eb ( φ b × ) l b + n GNSS = δr eIMU − C eb ( l b × ) φ b + n GNSS = δ ˆ r eib − C eb ( l b × ) φ b + n GNSS ≈ ˆ C eb J ρ br − C eb ( l b × ) φ b + n GNSS ≈ ˆ C eb J ρ br − ˆ C eb ( I − φ b × )( l b × ) φ b + n GNSS ≈ ˆ C eb J ρ br − ˆ C eb ( l b × ) φ b + n GNSS ≈ C eb ( I + φ b × ) J ρ br − C eb ( l b × ) φ b + n GNSS ≈ C eb J ρ br − C eb ( l b × ) φ b + n GNSS (134)Then the measurement matrix can be written as H = h − C eb ( l b × ) 0 C eb i (135)where C eb can be replaced by ˆ C eb when implement the algorithm, because the resulting error can beeliminated by a small second order quantity. Remark 4
It is worth noting that all the left Jacobian matrix J can be approximated as J ≈ I × if || φ b || is small enough. SE (3) based EKF for ECEF Navigation with Estimated Body Frame Attitude Error Considering the measurements of the GNSS are left-invariant observations for the world-centricobserver, we first give the left-invariant error state differential equations. The left-invariant error isdefined as η L = ˜ X − X = ˜ C be − ˜ v bib − ˜ r bib C eb v eib r eib = ˜ C be C eb ˜ C be v eib − ˜ v bib ˜ C be r eib − ˜ r bib (136)According to the map form the Lie algebra to the Lie group, the error states of attitude, velocity,and position can de derived as ˜ C be C eb = exp G ( φ b × ) ≈ I + φ b × η Lv = J ρ bv = ˜ C be v eib − ˜ v bib = ˜ C be v eib − ˜ C be ˜ v eib = ˜ C be ( v eib − ˜ v eib ) = − ˜ C be δv eib η Lr = J ρ br = ˜ C be r eib − ˜ r bib = ˜ C be r eib − ˜ C be ˜ r eib = ˜ C be ( r eib − ˜ r eib ) = − ˜ C be δr eib (137)35eanwhile, the left-invariant error satisfies that η L = exp G ( φ b × ) J ρ bv J ρ br = exp G ( φ b ) × ρ bv ρ br = exp G Λ φ b ρ bv ρ br (138)where φ b is the attitude error state, J ρ bv is the new definition of velocity error state, J ρ br is the newdefinition of position error state; J is the left Jacobian matrix given in equation(36).The differential equation of the attitude error state is given as ddt ( ˜ C be C eb ) = ˙˜ C be C eb + ˜ C be ˙ C eb = h ˜ C be (˜ ω eie × ) − (˜ ω bib × ) ˜ C be i C eb + ˜ C be (cid:2) C eb ( ω bib × ) − ( ω eie × ) C eb (cid:3) = ˜ C be ( ω eie × ) C eb − (˜ ω bib × ) ˜ C be C eb + ˜ C be C eb ( ω bib × ) − ˜ C be ( ω eie × ) C eb ≈ − (˜ ω bib × )( I + φ b × ) + ( I + φ b × )((˜ ω bib − δω bib ) × )= − (˜ ω bib × )( φ b × ) − ( δω bib ) × +( φ b × )(˜ ω bib × ) − φ b × ( δω bib ) ×≈ ( φ b × ˜ ω bib ) × − δω bib × = ( φ b × ˜ ω bib ) × − ( δb bg + w bg ) × (139)where the angular velocity error of the earth’s rotation can be neglected, i.e., ˆ ω eie = ω eie ; andsecond order small quantity ( φ b × )( δω bib × ) is also neglected. Therefore, the equation(128) can besimplified as ˙ φ b = φ b × ˜ ω bib − δω bib = − ˜ ω bib × φ b − δω bib = − ˜ ω bib × φ b − δb bg − w bg (140)The differential equation of the velocity error state is given as ddt ( J ρ bv ) = − ˙˜ C be δv eib + ˜ C be ( ˙ v eib − ˙˜ v eib )= − h ˜ C be (˜ ω eie × ) − (˜ ω bib × ) ˜ C be i δv eib + ˜ C be (cid:16)(cid:2) ( − ω eie × ) v eib + C eb f b + G eib (cid:3) − h ( − ˜ ω eie × )˜ v eib + ˜ C eb ˜ f b + ˜ G eib i(cid:17) = − ˜ C be ( ω eie × ) δv eib + (˜ ω bib × ) ˜ C be δv eib + ˜ C be C eb f b − ˜ f b − ˜ C be ω eie × ( v eib − ˜ v eib )+ ˜ C be ( G eib − ˜ G eib ) ≈ − (˜ ω bib × ) J ρ bv + ( I + φ b × )( ˜ f b − δb ba − w ba ) − ˜ f b + ˜ C be ( G eib − ˜ G eib )= − (˜ ω bib × ) J ρ bv + φ b × ˜ f b − φ b × δf b + ˜ C be ( G eib − ˜ G eib ) − δf b ≈ − ( ω bib × ) J ρ bv − f b × φ b + ˜ C be ( G eib − ˜ G eib ) + δf b = − ( ω bib × ) J ρ bv − f b × φ b + ˜ C be ( G eib − ˜ G eib ) − δb ba − w ba (141)36here the second order small quantity φ b × δf b is neglected; and as G eib can be approximated asconstant, ˜ C be ( G eib − ˜ G eib ) can also be neglected.In the same way, the differential equation of the position error state is given as ddt ( J ρ br ) = − ˙˜ C be δr eib + ˜ C be ( ˙ r eib − ˙˜ r eib )= − h ˜ C be ( ω eie × ) − (˜ ω bib × ) ˜ C be i δr eib + ˜ C be ([( − ω eie × ) r eib + v eib ] − [( − ˜ ω eie × )˜ r eib + ˜ v eib ])= − ˜ C be ( ω eie × ) δr eib + (˜ ω bib × ) ˜ C be δr eib − ˜ C be ω eie × ( r eib − ˜ r eib ) + ˜ C be ( v eib − ˜ v eib )= − ˜ ω bib × J ρ br + J ρ bv (142)Thus, the inertial-integrated error state dynamic equation for the SE (3) based EKF can beobtained δ ˙ x = F δx + Gw (143)where F is the error state transition matrix; δx is the error state including the terms about bias; Gis the noise driven matrix. Their definition is given as δx = φ b J ρ bv J ρ br δb bg δb ba , F = − ˜ ω bib × − I × − ˜ f b × − ˜ ω bib × − I × I × − ˜ ω bib × − τ g
00 0 0 0 − τ a ,G = − I × − I × I ×
00 0 0 I × , w = w bg w ba w bb g w bb a (144) SE (3) based EKF measurement equation If the lever arm error is taken into account, the measurement error vector is expressed in the ECEFframe as the difference between the position calculated by GNSS and the position calculated by37INS: δz r = ˆ r eGNSS − ˆ r eGINS = r eGNSS + n GNSS − (ˆ r eIMU + ˆ C eb l b ) ≈ − r eIMU − δr eIMU − C eb ( I − φ b × ) l b + r eGNSS + n GNSS = − r eIMU − C eb l b + r eGNSS − δr eIMU + C eb ( φ b × ) l b + n GNSS = − δr eIMU − C eb ( l b × ) φ b + n GNSS = − δ ˆ r eib − C eb ( l b × ) φ b + n GNSS ≈ ˜ C eb J ρ br − C eb ( l b × ) φ b + n GNSS ≈ ˜ C eb J ρ br − ˜ C eb ( I − φ b × )( l b × ) φ b + n GNSS ≈ ˜ C eb J ρ br − ˜ C eb ( l b × ) φ b + n GNSS ≈ C eb ( I + φ b × ) J ρ br − C eb ( l b × ) φ b + n GNSS ≈ C eb J ρ br − C eb ( l b × ) φ b + n GNSS (145)Then the measurement matrix can be written as H = h − ˜ C eb ( l b × ) 0 ˜ C eb i (146)where ˜ C eb can be replaced by C eb when implement the algorithm, because the resulting error can beeliminated by a small second order quantity. Remark 5
The equivalence of the SE (3) based measurement equation and the left invariant EKFmeasurement euation can be proofed similar to Subsection 6.2. From the above two equivalent re-lationships we can draw the conclusion that the error definition η = ˜ X − X is more reasonable forthe global navigation such as GNSS and 5G whose measurements have the left-invariant observa-tion form. Remark 6
The equivalence of the left-invariant EKF and the SE (3) based EKF can be verifiedin the same way and all the properties and conclusions shown in this manuscript can be obtainedand proved. The detailed derivation will be given soon. Especially, the right-invariant form of the SE (3) based EKF is suitable for the local navigation such as vision, lidar, whose measurementshave the right-invariant observation form. Moreover, it is more reasonable to define the error asthe estimated state multiplies the inverse of state, i.e., η = ˆ X X − . Remark 7
It is obvious the error state dynamic equation of SE (3) based EKF for ECEF nav-igation is more concise than the error state dynamic of SE (3) based EKF for NED navigation. owever, the SE (3) based EKF for NED navigation depends on the mechanization of GINS inthe NED frame and the SE (3) based EKF for ECEF navigation which also depends on the mech-anization of GINS in the NED frame and leads to the transformation of the mechanization resultfrom the NED frame to the ECEF frame. SE (3) based smoothing algorithm Since the SE (3) based EKF is easier to understand and its formulation is more intuitive than theInvariant EKF, we propose the SE (3) based smoothing algorithm which is essentially an applica-tion of the SE (3) based EKF. Our formulation is simple and easy to understand and different tothe invariant RTS smoother [15].As the SE (3) based EKF is more simple than the invariant-EKF, the SE (3) based EKFimplement the smoothing procedure as the RTS smoothing. The only difference lies in the fullstate update procedure, which need the matrix exponential map and multiplication operation onmatrix Lie group.
9. Conclusions
In this paper, SE (3) based extended Kalman filter is derived from the perspective of matrix Liegroup. This is nature and reasonable, consequently leads to common error representation for theinertial-integrated navigation system. The major contribution of this paper is the complete devel-opment of state error kinematics equations where the estimated state error is defined in either thetrue navigation frame or in the estimated navigation frame. The group-affine property of the dy-namics is verified. The attitude error, the velocity error, and the position error are defined on thecommon frame. The experiments show that the proposed SE (3) is robust for initial-integratednavigation with large misaligned angle. In the future, similar to the SE(3) based EKF for attitudeestimation [7], the biases of the accelerometer and gyroscope can be considered to incorporateinto matrix Lie group SE (3) so that the orientation error can be considered for the biases in theinertial-integrated navigation. Furthermore, the theory is applied to more inertial-integrated nav-igation applications such as initial alignment [16], tightly couple integration, filter-based SLAM,etc. 39 cknowledgement This research was supported by a grant from the National Key Research and Development Programof China (2018YFB1305001). We express thanks to GNSS Research Center, Wuhan University.
References [1] M. P. Whittaker and J. L. Crassidis, “Linearized analysis of inertial navigation employingcommon frame error representations,” in , 2018, p. 1600.[2] ——, “Inertial navigation employing common frame error representations,” in , 2017, p. 1031.[3] K. Li, L. Chang, and Y. Chen, “Common frame based unscented quaternion estimator forinertial-integrated navigation,”
IEEE/ASME Transactions on Mechatronics , vol. 23, no. 5,pp. 2413–2423, 2018.[4] M. Wang, W. Wu, P. Zhou, and X. He, “State transformation extended kalman filter forgps/sins tightly coupled integration,”
Gps Solutions , vol. 22, no. 4, p. 112, 2018.[5] M. Wang, W. Wu, X. He, Y. Li, and X. Pan, “Consistent st-ekf for long distance land ve-hicle navigation based on sins/od integration,”
IEEE Transactions on Vehicular Technology ,vol. 68, no. 11, pp. 10 525–10 534, 2019.[6] M. Wang, W. Wu, X. He, and X. Pan, “Further explanation and application of state transfor-mation extended kalman filter,”
Journal of Chinese Inertial Technology , vol. 27, no. 4, pp.499–504,509, 2019.[7] L. Chang, “Se(3) based extended kalman filter for spacecraft attitude estimation,”
Journal ofChinese Inertial Technology , vol. 28, no. 4, pp. 499–504,550, 2020.[8] M. P. Whittaker, “Inertial navigation employing a common frame error definition,” Ph.D.dissertation, State University of New York at Buffalo, 2019.409] A. Barrau and S. Bonnabel, “The invariant extended kalman filter as a stable observer,”