Aircraft motion state determination method based on Lie group unscented Kalman filtering

Abstract

The embodiment of the invention provides an aircraft motion state determination method based on Lie group unscented Kalman filtering, and the method comprises the steps: building a state model of a target aircraft, and a measurement model of the target aircraft; obtaining a final Lie group state estimation value of the target aircraft through iterative calculation according to the state model and the measurement model; and determining the motion state of the target aircraft according to the final Lie group state estimation value. According to the technical scheme, a Lie group unscented Kalman filtering navigation framework based on external measurement information is provided, specific forms of sigma point error propagation on Lie algebra and state updating on the Lie group are given, and a self-adaptive updating method of a state noise covariance matrix and a measurement noise covariance matrix is designed. Therefore, the self-adaptive Lie group unscented Kalman filtering method is provided, and the multi-sensor multi-beacon target state estimation precision is greatly improved by means of the self-adaptive Lie group unscented Kalman filtering method.

Description

Technical Field

[0001] The present invention relates to the field of navigation technology, and in particular to a method for estimating the motion state of an aircraft target, and more particularly to a method for determining the motion state of an aircraft based on a Lie group unscented Kalman filter. Background Art

[0002] Navigation and positioning of aircraft targets is a core technology in strategic deployment, military strikes, and space resource exploration. Methods for aircraft target navigation and positioning are primarily categorized into inertial navigation and navigation based on external measurement information. With the advancement of sensor measurement accuracy, navigation and positioning methods based on multi-sensor measurement systems have become an important approach for obtaining high-precision target trajectories. This approach utilizes multiple sensors to obtain measurement information (i.e., measurement values) from multiple beacons mounted on the target, thereby determining the target state. However, the observability theory for multi-sensor, multi-beacon target navigation systems is still incomplete. The high maneuverability and complex rigid-body motion characteristics make it difficult to accurately construct a target state model. The resulting tracking position inconsistency errors become a bottleneck restricting high-precision target positioning. Furthermore, existing aircraft target state estimation algorithms based on external measurement information struggle to ensure consistency due to factors such as target state dimensionality, station geometry, linearization errors, and complex positioning environments. Therefore, research on target observability theory and high-precision estimation algorithms is a crucial approach to improving target estimation accuracy.

[0003] Currently, research on multi-sensor, multi-beacon target state models primarily focuses on kinematic and dynamic modeling based on point-mass models, including polynomial models, Singer models, "current" statistical models, and highly maneuverable Jerk models. Spline models offer the advantage of full-time modeling, but the large number of spline nodes reduces modeling efficiency. To overcome the limitations of single models, interactive multi-model algorithms have emerged. However, constructing a model set and determining state transition probabilities remain challenging issues that affect target positioning accuracy. The complex rigid-body motion of multi-beacon targets poses significant challenges to target state model construction. Point-mass models fail to account for target attitude variations, resulting in misalignment between the tracking position of the external measurement device and the target's center of mass, leading to reduced positioning accuracy. Furthermore, existing navigation methods based on multi-sensor measurement systems fail to account for measurement inaccuracies caused by the coupled translational and rotational effects of the target. Furthermore, the lack of observability conditions for multi-sensor, multi-beacon targets and the incompleteness of observability theory further restrict positioning accuracy. Therefore, it is necessary to establish a new external measurement information navigation and positioning framework based on an integrated translational and rotational model of aircraft targets, and then conduct theoretical research on the observability of multi-sensor, multi-beacon targets.

[0004] In addition to the aforementioned point-mass model, modeling the target's attitude using quaternions, treating the target as a rigid body, has been widely developed and applied in target pose estimation. However, in the navigation framework based on extrinsic information, the positioning accuracy of quaternion-based point-by-point measurement solutions depends on measurement accuracy, state dimension, and the geometric dilution of precision (GDOP) of the station layout. Positioning accuracy is lower when the state dimension is high and the station layout is poor. The quaternion-based extended Kalman filter method has also been applied to spacecraft inertial navigation systems and satellite vision navigation systems. Another important development is the unscented Kalman filter attitude estimation method, which uses unscented transformations and sigma point propagation to estimate the target quaternion. Furthermore, quaternion-based methods such as the particle filter (PF), nonlinear predictive filter (NPF), and iterative extended Kalman filter (IEKF) have also been successfully applied to target pose estimation. However, in extrinsic information navigation systems, linearization truncation error remains a significant factor. On the other hand, the target's position, velocity, angular velocity and other states belong to the Euclidean space, while the posture belongs to the Lie group SO(3) space. The dynamic equations evolve in the nonlinear manifold space. The above estimation method based on the definition of state error in Euclidean space does not consider the inconsistency of the state space. The state error and the error covariance matrix are both propagated in the Euclidean space. The error propagation matrix is ​​related to the current uncertain state. The positive feedback of the error reduces the filtering accuracy and algorithm consistency.

[0005] In order to solve this problem, Lie group theory has received great attention in target pose estimation in recent years. The target pose modeling method based on the rotation matrix improves the algorithm tracking accuracy from the perspective of state space consistency. The fundamental reason is that the rotation matrix and the homogeneous transformation matrix satisfy the good properties of the matrix Lie group. The error covariance matrix propagates in the Lie algebra of the tangent space of the Lie group and converts to and from the Lie group space through the exponential mapping, ensuring the consistency of the state space and the algorithm. Barrau and Bonnabel proposed the invariant extended Kalman filter (InEKF) based on the inertial navigation framework. The main contribution is to achieve the independence of the error transfer matrix and the current state by reconstructing the state error, thereby improving the consistency of the algorithm. By constructing the mixed state error equation of the translation and rotation of multiple beacon targets and the exponential update equation of the state variable, it is theoretically proved that the external information navigation system does not satisfy the above invariance. With the introduction of the concentrated Gaussian distribution on the Lie group, the unscented Kalman filter method based on the state of the Lie group space such as SO(3), SE(3), SE2(3) has developed rapidly, in which the sigma point is generated in the Lie algebra and the state is propagated in the Lie group space. In addition, the state uncertainty in the Lie group space is defined by exponential mappings of Gaussian distributions on Lie algebras, and a Lie group unscented Kalman filter (UKF-LG) with different measurement equations is considered. However, most of the above methods are targeted at inertial navigation systems. In external information navigation systems, the accuracy characteristics of the measurement equipment are unknown, and the positioning environment is complex and changeable, resulting in the state noise covariance matrix (SNCM) and measurement noise covariance matrix (MNCM) exhibiting time-varying and uncertainty. The inaccurate prior S in the above algorithms can improve the accuracy of the aircraft target in the multi-sensor NCM and MNCM, reducing the state estimation accuracy and even causing filter divergence.

[0006] In summary, the accuracy of the estimation method under the multi-sensor multi-beacon target state is a problem that needs to be solved. Summary of the Invention

[0007] An embodiment of the present invention provides a method for determining the motion state of an aircraft based on a Lie group unscented Kalman filter, so as to improve the estimation accuracy of an aircraft target in a multi-sensor and multi-beacon target state.

[0008] To achieve the above-mentioned purpose, an embodiment of the present invention provides a method for determining the motion state of an aircraft based on a Lie group unscented Kalman filter, comprising: establishing a state model of a target aircraft and a measurement model for the target aircraft; obtaining a final Lie group state estimation value of the target aircraft through iterative calculation based on the state model and the measurement model; and determining the motion state of the target aircraft based on the final Lie group state estimation value.

[0009] The above technical solution has the following beneficial effects:

[0010] The present invention studies the adaptive Lie group unscented Kalman filtering method based on distance and radial velocity measurement, and realizes the high-precision position and attitude estimation of multi-sensor multi-beacon targets. Since the observability theory is imperfect in the existing multi-sensor multi-beacon target observation system, the position inconsistency error causes the positioning accuracy to decrease. Therefore, the present invention constructs an integrated model of target translation and rotation, and derives the necessary and sufficient conditions for the observability of multi-sensor multi-beacon targets. At the same time, in order to solve the problem that the traditional estimation method is difficult to ensure consistency due to the influence of state dimension and error positive feedback, the present invention constructs A Lie group unscented Kalman filter navigation framework based on external measurement information is proposed. Based on the Lie group SE5(3) spatial state, the specific forms of error covariance propagation and state update are derived through sigma point sampling and exponential mapping in Lie algebra, which improves the state space consistency. This method can significantly improve the position and attitude estimation accuracy of multi-beacon targets, and provides a theoretical basis for multi-beacon target positioning based on external measurement information.

[0011] In addition, the present invention also has the following characteristics:

[0012] The present invention also proposes SNCM and MNCM adaptive estimation methods, and further proposes an adaptive Lie group unscented Kalman filtering method, which improves the robustness to complex positioning environments and inaccurate prior noise covariance. BRIEF DESCRIPTION OF THE DRAWINGS

[0013] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0014] Figure 1 The present invention is a flow chart of a method for determining the motion state of an aircraft based on Lie group unscented Kalman filtering. DETAILED DESCRIPTION

[0015] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0016] like Figure 1 As shown, an embodiment of the present invention provides a method for determining the motion state of an aircraft based on a Lie group unscented Kalman filter, comprising:

[0017] S101, establishing a state model of a target aircraft and a measurement model for the target aircraft;

[0018] S102, obtaining a final Lie group state estimate of the target aircraft through iterative calculation based on the state model and the measurement model;

[0019] S103: Determine the motion state of the target aircraft according to the final Lie group state estimation value.

[0020] In this application, although the existing technology can be used to obtain the measurement values ​​of the target aircraft (that is, using sensors to measure the distance between it and the target aircraft, and the radial velocity of the target aircraft), and the measurement values ​​can be used to solve the state of the target aircraft (position, speed, etc.), this solution is erroneous. Therefore, it is necessary to establish a state model of the target aircraft, based on the Lie group and combined with the measurement values, to obtain the Lie group state estimation value, so that the final result of the solution is more accurate and better meets actual needs.

[0021] Furthermore, the state model is in the form of:

[0022] χ k =f χ (χ k-1 ,ε k-1 ),

[0023] Where χ represents the estimated value of the Lie group state, k represents the current moment, k-1 represents the previous moment, ε is the Gaussian white noise in the Euclidean space, and Q represents the state noise covariance matrix, 0 q Represents the q-dimensional zero vector (that is, the mean of the noise is zero)

[0024] The measurement model is of the form:

[0025] z k =h χ (χ k )+υ k

[0026]

[0027] Where z represents the measurement information of the target aircraft, R represents the measurement noise covariance matrix, υ represents the measurement noise, m represents the number of sensors used to measure the target aircraft, h represents a function, h χ (χ k ) means that the distance information is a function of the target aircraft's state (position). Here, each sensor measures two values, one for distance and one for radial velocity, so there are 2*m measurements in total.

[0028] Furthermore, the step S102 includes:

[0029] S1021. Determine, based on prior information, an initial value of a Lie group state prediction value, an initial value of a state noise covariance matrix, and an initial value of a measurement noise covariance matrix, wherein the Lie group state includes motion state information of the target aircraft;

[0030] S1022, setting the initial value of the number of iterations k to k=1;

[0031] S1023. Using the initial value of the Lie group state prediction value as the Lie group state prediction value at time k-1, using the initial value of the state noise covariance matrix as the state noise covariance matrix at time k-1, and using the initial value of the measurement noise covariance matrix as the measurement noise covariance matrix at time k-1;

[0032] S1024. Obtain a Lie group state prediction value at time k according to the Lie group state prediction value at time k-1 and the state noise covariance matrix at time k-1;

[0033] S1025. Obtain a Lie group state estimation value at time k based on the Lie group state prediction value at time k and the measurement noise covariance matrix at time k-1;

[0034] S1026, determining a k-time state noise covariance matrix and a k-time measurement noise covariance matrix according to the k-time Lie group state prediction value and the k-time Lie group state estimation value;

[0035] S1027, update the number of iterations k = k + 1;

[0036] S1028, when k<k max When , the above iterative calculation process S1024-S1027 is repeatedly executed;

[0037] S1029. Use the last obtained Lie group state estimation value at time k as the final Lie group state estimation value.

[0038] Furthermore, the step S1024 specifically includes:

[0039] S10242. Perform one-step prediction based on the k-1 moment Lie group state prediction value, the k-1 moment state noise covariance matrix, and the state model to obtain the k- moment Lie group state prediction value.

[0040] Furthermore, after step S10241, the method further includes:

[0041] S10242: Determine a first scaling parameter using a preset state dimension, a scaling coefficient, and a noise dimension;

[0042] S10243. Determine the weight of a first augmented sigma point using the first scaling parameter;

[0043] S10244: Obtain a first augmented covariance matrix according to the k-1 moment Lie group state prediction value and the k-1 moment state noise covariance matrix, and determine the first augmented sigma point by using the first augmented covariance matrix. ；

[0044] S10245: Split the first augmented sigma point to obtain a first Lie algebraic error sigma point and a state noise sigma point;

[0045] S10246. Calculate a Lie group state error based on the Lie group state prediction value at time k, the first Lie algebraic error sigma point, and the state noise sigma point;

[0046] S10247. According to the weight of the first augmented sigma point and the Lie group state error, a predicted value of the Lie algebraic error covariance matrix at time k is calculated by using a logarithmic mapping and weighting method.

[0047] Furthermore, the step S1025 specifically includes:

[0048] S10251. Determine a second scaling parameter using the state dimension and a preset measurement dimension;

[0049] S10252. Determine the weight of the second augmented sigma point using the second scaling parameter;

[0050] S10253: Obtain a second augmented covariance matrix according to the Lie group state prediction value at time k and the measurement noise covariance matrix at time k-1, and determine the second augmented sigma point through the second augmented covariance matrix;

[0051] S10254: Split the second augmented sigma point to obtain a second Lie algebraic error sigma point and a measurement noise sigma point;

[0052] S10255: Determine a measurement prediction value of the second augmented sigma point based on the Lie group state prediction value at time k, the second Lie algebraic error sigma point, the measurement noise sigma point, and the measurement model;

[0053] S10256: averaging the second augmented sigma points to obtain a second augmented sigma point mean vector, and then performing a weighted summation on the measurement prediction value using the weight of the second augmented sigma point to obtain a measurement prediction mean, an error covariance of the measurement prediction value, and an error covariance between the second augmented sigma point mean vector and the measurement prediction value;

[0054] S10257. Obtain a Lie algebra error estimate at time k based on the measurement prediction mean, the error covariance of the measurement prediction value, the error covariance between the second augmented sigma point mean vector and the measurement prediction value, and the measurement information.

[0055] S10258. Obtain the estimated value of the Lie group state at time k according to the predicted value of the Lie group state at time k and the estimated value of the Lie algebra error at time k.

[0056] Furthermore, after step S10258, the method further includes:

[0057] S10259. Update the predicted value of the Lie algebraic error covariance matrix at time k according to the estimated value of the Lie algebraic error at time k and the measurement noise covariance matrix at time k-1 to obtain the Lie algebraic error covariance matrix at time k.

[0058] The purpose of calculating the Lie algebra error covariance matrix at time k here is: this is a fixed output parameter, which is equivalent to obtaining not only the final Lie group state estimate, but also the Lie algebra error covariance matrix. This matrix describes the quality of the obtained Lie group state estimate, which is equivalent to the error of the Lie group state and can be used to evaluate the Lie group state estimate.

[0059] Furthermore, the step S1026 specifically includes:

[0060] S10261, converting the k-time Lie group state prediction value into the k-time Euclidean space state prediction value, and converting the k-time Lie group state estimation value into the k-time Euclidean space state estimation value;

[0061] S10262: Calculate the Euclidean space state prediction value at time k and the Euclidean space state estimation value at time k using a Kalman filter to obtain an error covariance matrix and a gain matrix for the Euclidean space state prediction value at time k;

[0062] S10263. Calculate the measurement residual of the Euclidean space state estimate at time k;

[0063] S10264: updating the k-1 moment measurement noise covariance matrix according to the measurement residual and the error covariance matrix of the Euclidean space state prediction value at moment k, thereby obtaining the k moment measurement noise covariance matrix;

[0064] S10265. Update the k-1 moment state noise covariance matrix according to the gain matrix, the measurement information, and the k- moment Euclidean space state prediction value, thereby obtaining the k- moment state noise covariance matrix.

[0065] The above method and related derivation process are described in detail below through a specific embodiment:

[0066] 1. Multi-sensor and multi-beacon target observability theory

[0067] In the multi-sensor measurement system of aircraft targets, the ground sensor X 0i =(x 0i ,y 0i ,z 0i ) T ,i=1,2,…,m receives multiple beacons X from the target 1j =(x 1j ,y 1j ,z 1j ) T ,j=1,2,…,n signals, and then obtain measurement information. Sensor coordinate system The origin is a certain sensor, X s Axis and Y s The axes point to the east and north respectively, Z s The axis is determined by the right-hand rule. Taking the target center of mass as the origin, X T Along the target's main axis, pointing to the target's direction of motion, Y T The axis is in the target longitudinal symmetry plane and perpendicular to the X T Axis, Z T The axes are determined by the right-hand rule. arrive The rotation matrix is ​​R AB In the target body coordinate system, the positions of the beacons relative to the center of mass are ρ i ,i=1,2,…,n. In a special case, the beacons are evenly distributed on a circle on the target surface with a radius of r and an angle of θ0 between two adjacent beacons.

[0068] 2. Target translation and rotation integrated state model

[0069] The sensor position vector of the target center of mass is X = (x, y, z) T , the speed is v=(vx ,v y ,v z ) T , the acceleration is a=(a x ,a y ,a z ) T In the absence of rolling control, the target performs free rolling motion, and the angular velocity of the target system is ω=(ω x ,ω y ,ω z ) T . Coordinate system Relative to The unit quaternion is

[0070]

[0071] Its meaning is coordinate system Rotation around the unit vector axis e Angle to the target body coordinate system. Then the rotation matrix R AB The relationship with q is

[0072]

[0073] Among them, I3 is the third-order unit matrix, To satisfy The antisymmetric matrix

[0074]

[0075] The conjugate of the unit quaternion q, i.e. its inverse, is

[0076] Let the principal inertia be I xx ,I yy and I zz , then the target inertia ratio p is

[0077]

[0078] According to the Euler equation, the dynamic equation of the target rolling motion is:

[0079]

[0080] in,

[0081]

[0082] Remember it The above expression is The kinematic equation of the target quaternion is

[0083]

[0084] in, Represents the quaternion product operation, defined as

[0085]

[0086] Define the quaternion error as

[0087]

[0088] Linearizing formula (5) yields:

[0089]

[0090] in

[0091]

[0092] Similarly, linearizing equation (7) yields:

[0093]

[0094] make represents the state quantity of the target with respect to the rotational motion. From equations (10) to (12), we can see that the linearized state equation is:

[0095]

[0096] Among them, ε r ~N(0,Q r ) is a Gaussian white noise sequence, and its covariance

[0097] Different translational motion models of targets will result in different state transfer matrices, but the observability analysis method of multi-beacon targets remains unchanged. Therefore, we take the quadratic polynomial model as an example to conduct theoretical research on the observability of multi-beacon targets. Let x t =(X T ,v T ,a T ) T Represents the state quantity of the target regarding translational motion, and its linearized state equation is:

[0098]

[0099] Among them, ε t ~N(0,Q t ) is a Gaussian white noise sequence, and its covariance

[0100] In summary, the system state variables of the target in Euclidean space are defined as:

[0101]

[0102] From the continuous-time systems (13) and (14), we can obtain the discrete system:

[0103] x k+1 =Φ k x k +ε k (16)

[0104] Among them, Φ k =Φ(t k ,T Δ ) is the state transfer matrix of the entire system, T Δ =t k+1 -t k is the sampling time. is the state noise. Considering that the target translation and rotation are independent of each other, and discretized from (13) and (14), we can know that:

[0105]

[0106] 3. Multi-sensor multi-beacon target measurement model

[0107] In multi-sensor measurement systems, distance and radial velocity measurements have high accuracy and are widely used in target positioning. Therefore, this paper constructs a multi-sensor distance and radial velocity measurement model for aircraft targets. At the same time, each sensor receives only one beacon signal and measures its distance and radial velocity. The observed value at time k is the distance and radial velocity information of m sensors. The measurement equation is:

[0108]

[0109] in, and represent the distance and radial velocity measurements of sensor i at time k, j i Indicates the beacon sequence number corresponding to sensor i. is the translational velocity of the beacon generated by the target rotation in the target system, and are the ranging error and speed measurement error of sensor i respectively. The specific derivation process is:

[0110] Assume that the unit vector of the target rotation axis is λ r , the angular velocity of rotation is but

[0111]

[0112] Let the translational velocity of beacon i caused by the rotational motion be v1i ,i=1,2,…,n, its direction is perpendicular to ρ i and λ r The plane where it is located is

[0113]

[0114] Taking into account

[0115]

[0116] And ω×ρ i Direction and v 1i Same, so

[0117] v 1i =ω×ρ i

[0118] Then the measurement model (18) can be obtained.

[0119] Then, according to equations (2) and (18), the discrete measurement model can be expressed as

[0120]

[0121] in is the range and radial velocity measurement noise, R k =diag(R 1k ,R 2k ) is the measurement noise covariance matrix.

[0122] Next we need to find the Jacobian matrix of the observation quantity to the state quantity.

[0123]

[0124] Among them L k is the direction cosine matrix from the beacon to the sensor measured at time k. Similarly, the Jacobian matrix of the radial velocity information for the target translation motion is

[0125]

[0126] Among them D k is the Jacobian matrix of the radial velocity information to the target position at time k.

[0127] For target rotation motion, note that Therefore

[0128] From formula (2), we can see

[0129]

[0130] Therefore, when δq satisfies ||δqv When ||<<1 and δq0≈1

[0131]

[0132] Taking into account Therefore, the relative distance information can be expressed as

[0133]

[0134] Also because therefore

[0135]

[0136] Among them, the equation Similarly, the radial velocity information can be expressed as

[0137]

[0138] therefore,

[0139]

[0140] Taking into account Then the radial velocity information can be expressed as

[0141]

[0142] Therefore, its effect on δω k The Jacobian matrix of

[0143]

[0144] Combining equations (20) to (29), the linearized measurement equation can be written as

[0145]

[0146] 4. Observability Analysis

[0147] Based on the aforementioned state model and measurement model, the observability of a multi-sensor, multi-beacon target is analyzed. When the system state at a given moment can be uniquely determined by the relative distance and radial velocity measurements at that moment, the system or state is said to be single-point observable. Within a certain interval of the target's motion, a system or state is said to be interval observable if, by applying state model constraints to the target and combining the relative distance and radial velocity information at multiple moments, the target's initial state within that interval can be uniquely determined.

[0148] (1) Single point observability

[0149] Definition 1: Observation quantity z at time k k For the target state xk The Jacobian matrix of When H k Null space Null(H k ) has only zero vector, namely H k ·λ=0 It is necessary that when λ=0, the system is observable at a single point.

[0150] Lemma 1: For a block matrix

[0151]

[0152] but A sufficient condition for

[0153]

[0154] That is, the matrix [L k S k ] and [L k J k ]The null space only has zero vectors. A necessary condition for

[0155]

[0156] The proof process of the column full rank condition of the block matrix:

[0157] If the matrix or [L k J k ] has a non-zero vector in its null space, that is, the columns are linearly dependent, then the matrix There must be a linear correlation, that is satisfy Therefore, the necessary condition is obviously met. Now let's prove the sufficient condition.

[0158] Using proof by contradiction, if There is a non-zero vector in the null space, that is, there is λ=(λ1,λ2,…,λ 12 ) T ≠0 12 Make

[0159]

[0160] λ (1) =(λ1,λ2,λ3,λ7,λ8,λ9) T

[0161] λ (2) =(λ4,λ5,λ6,λ 10 ,λ 11 ,λ 12 ) T (125)

[0162] By Null([L k S k ])={0},[L k S k ]·λ (1) =0 m , we know that λ (1) =06, then there exists λ (2) ≠06 makes [L k J k ]λ (2) =0 m Established, and Null([L k J k ])={0} is contradictory, so the sufficient condition holds.

[0163] Conclusion 1: The following conclusions can be drawn from the multi-sensor multi-beacon target observation system based on relative distance and radial velocity:

[0164] ① The target acceleration and inertia ratio cannot be observed at a single point;

[0165] ②The necessary and sufficient conditions for the single-point observability of target position, velocity, quaternion, and angular velocity are m ≥ 6 and the matrix Does not contain more than one all-zero column, i.e. 0 m-1 .

[0166] prove:

[0167] ① The target single point observation information does not contain acceleration and inertia ratio information, resulting in the matrix H k In the equation, the Jacobian matrix of the observed quantity for acceleration and inertia ratio is 0 2m×6 , H k The column is not full rank, that is There exists a nonzero vector such that a k With p k A single point is not observable.

[0168] ② The Jacobian matrix of the observation quantity for position, velocity, quaternion, and angular velocity is As shown in formula (31), when m < 6, it is easy to know that Right now The system is not full rank and cannot be observed. According to formula (29), the matrix J k It can be written as:

[0169]

[0170] Therefore, according to formulas (20) and (34), in the matrix [L k J k ]middle,

[0171]

[0172] When the matrix ρ * When it contains two or more all-zero columns, that is, At least two coordinate components are exactly the same, let like and Then from formula (35) we can know

[0173]

[0174] in, and Represents the matrix J k and L k The i-th column of is the matrix R AB (q k )The elements of row i and column j. Equation (36) shows that the matrix [L k J k ] columns are linearly related, similarly if The other two or three coordinate components are exactly the same, [L k J k ] is also not full column rank, according to formula (33), the matrix The necessary condition for the column to be full rank does not hold and the system is unobservable. In summary, the necessity of the above conditions holds.

[0175] From formula (25), we can see

[0176]

[0177] It is easy to see from equations (20), (25) and (29) that when the observation beacons are not all located in the station plane, L k ,S k ,J k On the other hand, similar to formula (36), when ρ * When it does not contain two or more all-zero columns, and Cannot be expressed as The linear combination of , from equations (20), (36) and (37), and and Linearly independent. Therefore, [L k S k ] and [L k J k ] are all full rank, and Lemma 1 shows that The column is full rank, the system is observable, and the sufficiency condition holds.

[0178] Note: From the above conclusion 1, we can know that the system is single-point observable if and only if the number of sensors m ≥ 6, and the position vector of the beacon observed by m sensors in this system is No more than one coordinate component is identical, meaning they are not on the same axis parallel to the coordinate axis. This conclusion also provides guidance for the placement of beacons on the target. All beacons are installed on the same axis parallel to the target's coordinate system. In this case, regardless of the number or spacing of beacons, a single point in the system is unobservable.

[0179] The above conditions ensure that a unique target position, velocity, quaternion, and angular velocity can be obtained based solely on the relative distance and radial velocity measurement information. However, in actual multi-sensor multi-beacon target observation systems, the accuracy of single-point estimation is difficult to improve due to factors such as station geometry design, measurement accuracy, and state variable dimension. Therefore, a more accurate tracking position correction and target positioning method should be designed by combining the target state model and measurement model.

[0180] (2) Interval Observability

[0181] Definition 2: The target is [t k ,t k+s ] is the observability matrix in the interval [t k ,t k+s ] can be observed, where Ξ is

[0182]

[0183] Conclusion 2: The above system has k ,t k+s The necessary and sufficient conditions for the interval to be observable are

[0184] Proof: According to formula (38), when When rank(Ξ)≤2(s+1)m<18, the system is obviously interval unobservable. The following proves The time system interval is observable.

[0185] The state equation of the continuous-time system is Where Φ=diag(Φ t ,Φ r ), Φ t and Φ r As in formula (13) and (14), i To j The discrete system state transfer matrix at sampling time is: in and Differential equations and The solution.

[0186] For the translational motion of the target, It can be seen that

[0187]

[0188] For target rotation motion,

[0189]

[0190] in,

[0191]

[0192] Easy to know Φ i,i =I 18 , combining equations (30) and (38),

[0193]

[0194] Among them, H k+i Φ k,k+i ,i=0,1,2,…,s is

[0195]

[0196] For the target translation motion part,

[0197]

[0198] If exists Make (H k+i Φ k,k+i ) t λ=0 2m , that is, λ∈Null((H k+i Φ k,k+i ) t ). Then according to formula (44), λ has the following form:

[0199]

[0200] but If and only if λ1=λ2=λ3=0, that is, λ= 09 .therefore, The columns are linearly independent. Similarly, from formula (41), when j>i, is a full rank square matrix and is related to the rotation matrix and angular velocity at time k+i. Similar to equation (45), Null(Ξ)={0 18}, so the Ξ columns are linearly independent, and the system is [t k ,t k+s ] interval is observable.

[0201] Note: Conclusion 2 above shows that even if m sensors are in the interval [t k ,t k+s ] is the same beacon on the target. As long as the relative number of sensors and sampling points is satisfied, the system is still observable. Obviously, the conditions in Conclusion 1 include the conditions in Conclusion 2, that is, the target state model relaxes the conditions for system observability.

[0202] Aircraft target state estimation based on adaptive Lie group UKF:

[0203] Lie group UKF introduces Gaussian distribution on Lie group through exponential mapping

[41] , the error covariance is transferred by using the sigma point on the Lie algebra, which does not need to satisfy the invariance of the error transfer equation and improves the consistency of the estimation method from the perspective of state space consistency.

[0204] 5.1 Lie Group UKF

[0205] Based on the multi-beacon target state model, the Lie group UKF state is defined as the element on the Lie group SE5(3)

[0206]

[0207] From equations (13) and (14), we can get the system state equation based on Lie group, which can be expressed as

[0208] χ k =f χ (χ k-1 ,ε k-1 ) (102)

[0209] in is Gaussian white noise in Euclidean space. According to formula (18), its measurement equation is recorded as

[0210]

[0211] Let the left invariant error The corresponding Lie algebra is ξ, that is, η = exp(Λ(ξ)), which defines the left invariant Gaussian distribution on the Lie group for

[0212]

[0213] Where ξ is a Gaussian distribution with covariance matrix P on Euclidean space. Similarly, the right invariant Gaussian distribution on Lie group can be defined as

[0214]

[0215] Taking the left invariant Gaussian distribution as an example, assuming that the state distribution at each moment is The purpose of Lie group UKF is to estimate the state at each moment and error covariance P k The Lie group UKF based on the left invariant Gaussian distribution (LUKF-LG) is divided into two steps:

[0216] 1) State prediction

[0217] Assume that the state distribution at time k-1 is Estimate the state distribution using it as known prior information Right now and One-step prediction of the state can be obtained through the state model

[0218]

[0219] The prediction error covariance is obtained through the Lie algebra error ξ k-1 and ε k-1 Substituting the relationship between Lie algebra error and state into equation (102) yields

[0220]

[0221] Right now

[0222]

[0223] pass and Generate sigma points and corresponding weights, substitute them into formula (108) and obtain by weighting the corresponding logarithmic mapping See Algorithm 1 for detailed steps.

[0224] 2) Measurement update

[0225] Measurement update can be reduced to a Bayesian estimation problem, that is, solving the posterior probability distribution

[0226]

[0227] The prior information is

[0228]

[0229] Taking into account as well as Use the following UT transformation to approximate the above posterior probability distribution, first generate the sigma point Substituting it into the measurement equation, we get

[0230]

[0231] Calculate the predicted value of the measurement by weight Measurement error covariance P zz and the cross-covariance P αz Then we can get the posterior distribution of ξ in

[0232]

[0233] Therefore, the above posterior distribution is Mapping its exponential to the Lie group, taking into account The system status can be updated as follows:

[0234]

[0235] This forms a closed loop of LUKF-LG. Similarly, the steps of the RUKF-LG algorithm can be obtained using the right invariant Gaussian distribution.

[0236] 5.2 Adaptive Lie Group UKF

[0237] In practical target observation systems based on relative range and radial velocity, the complex and changing positioning environment, the uncertainty of the target's motion state, and the unknown measurement accuracy of external measurement equipment all affect the state noise covariance matrix (SNCM) and the measurement noise covariance matrix (MNCM), resulting in a decrease in positioning accuracy. To address this issue, this paper proposes an adaptive Lie group UKF method (AUKF-LG). At each iteration, the Lie group state estimates obtained by UKF-LG are converted to EKF states. The SNCM and MNCM are adaptively estimated using state updates and measurement residuals in the EKF framework. The updated SNCM and MNCM are then fed back into the Lie group UKF method. This method improves robustness to these factors and inaccurate prior information, thereby enhancing target positioning accuracy and the consistency of the filtering method.

[0238] 1) MNCM adaptive estimation based on more accurate measurement residuals.

[0239] In the UKF-LG state equation (102), state transfer is achieved through χ k With the EKF state x k The transformation is realized, so the UKF-LG and EKF measurement noise ε k The same. By constructing the measurement residual sequence Taking the covariance matrix on both sides of the above zero-mean sequence, we can get This enables online estimation of MNCM However, this method does not guarantee R k The non-negative definiteness of The residual sequence of

[0240]

[0241] It can better reflect the characteristics of measurement noise. Therefore, based on the above zero-mean residual sequence, the covariance of both sides is obtained

[0242]

[0243] Therefore, through The online estimation of MNCM can be realized, where and Respectively by and According to the relationship between the rotation matrix and the quaternion, The expectation of is approximately the sequence The mean of .

[0244] In addition, to avoid R k The drastic changes in inertia factor κ are introduced. The larger κ is, the greater the R k The greater the inertia, the slower the response to the influence of complex factors. k The update method becomes

[0245]

[0246] 2) SNCM adaptive estimation based on state update

[0247] According to formula (16), the process noise ε k-1 =x k -Φ k-1 x k-1 ,therefore

[0248]

[0249] Taking the covariance on both sides of the above formula is

[0250]

[0251] Similarly, the inertia factor κ is introduced to make Q change smoothly, and the above formula becomes

[0252]

[0253] In summary, by combining the proposed SNCM and MNCM adaptive estimation methods with the Lie group UKF based on the left-invariant Gaussian distribution and the right-invariant Gaussian distribution in SE5(3), we designed the adaptive left-invariant Lie group UKF (ALUKF-LG) and the adaptive right-invariant Lie group UKF (ARUKF-LG), thereby realizing the aircraft target state estimation based on relative range and radial velocity information. The specific implementation process of the proposed ALUKF-LG process is shown in Table 1. Similarly, the process and implementation process of ARUKF-LG can be obtained.

[0254] Table 1. Specific implementation process of adaptive left invariant Lie group

[0255]

[0256]

[0257]

[0258] Note: col(·) j Represents the jth column of the matrix; the update process of MNCM and SNCM requires the error covariance matrix of the Euclidean space state. To distinguish them, the Lie algebra error covariance matrix is ​​recorded as ξ P k .

[0259] 6 Simulation and Analysis

[0260] In order to verify the proposed adaptive Lie group unscented Kalman filter method, a Monte Carlo simulation experiment of navigation and positioning based on relative distance and radial velocity information was carried out. The target state estimation results of the measurement solution (MS), quaternion-based extended Kalman filter (QEKF), left-invariant extended Kalman filter (LInEKF), left-invariant Lie group unscented Kalman filter (LUKF-LG), and the proposed ALUKF-LG and ARUKF-LG were compared.

[0261] 6.1 Simulation Scenario

[0262] (1) Translational motion

[0263] The multi-sensor multi-beacon target navigation and positioning system based on relative distance and radial velocity information consists of ground sensors, targets, and beacons on the targets. The number of ground sensors is set to m = 10. The target trajectory is generated by simulation according to Equation (14) with a sampling frequency of 100 Hz. Considering the undulating terrain, the sensors are generally not strictly located in the same plane.

[0264] (2) Rotational motion

[0265] Set the number of beacons on the target n = 6, the target radius r = 1.15m, and each sensor obtains the relative distance and radial velocity information of a beacon within t∈[0,5s]. According to equations (5) and (7), the simulation generates the quaternion of the target body coordinate system relative to the sensor system and the angular velocity of the target body system, where (c) is the Roll, Pitch, and Yaw converted from the quaternion. The target moment of inertia is set to I xx =1000kg·m 2 ,I yy =800kg·m 2 ,I zz =600kg·m2 .

[0266] (3) Algorithm parameter setting

[0267] The state noise covariance matrix SNCM of the target translation motion is set as The SNCM of the rotational motion is set to Q r =diag([0.01°·I 3×1 ; 1rad / s·I 3×1 ;0 3×1 ]). The measurement noise covariance matrix is ​​set to R = diag([0.1m·I 10×1 ;0.01m / s·I 10×1 ]), and the relative distance and radial velocity measurement noise are 0.1m and 0.01m / s respectively. The initial error covariance matrix of the QEKF algorithm is set to P0=diag(1·I 18×1 In the LInEKF, LUKF-LG, and the proposed ALUKF-LG and ARUKF-LG methods, the initial rotation matrix estimate is set to I3, and the initial estimates of other parameters are the same as those of the MS and QEKF methods.

[0268] In the LInEKF, LUKF-LG and the proposed ALUKF-LG and ARUKF-LG methods, the state error covariance matrix is ​​propagated on the Lie algebra, reflecting the uncertainty of the state error on the Lie algebra. The initial Lie algebra error covariance matrix is ​​set to ξ P0=P0=diag(1·I 18×1 ), where P0 is the initial error covariance matrix in Euclidean space. To verify the effectiveness of the proposed adaptive SNCM and MNCM estimation methods, in all methods, the initial SNCM and MNCM are set to Q0 = diag([0.1·Q r ;0.1·Q t ]) and R0=10·R=diag([1m·I 10×1 ;0.1m / s·I 10×1 ]). The inertia factor in the ALUKF-LG and ARUKF-LG methods is set to κ = 0.3.

[0269] 6.2 Simulation results and analysis

[0270] The above methods were used to perform 100 Monte Carlo simulation experiments on the target state estimation. The quaternion and rotation matrix estimates of each method were converted into Euler angles (Roll, Pitch, Yaw) for comparison. The root mean square error (RMSE) of each state variable at each moment and the total root mean square error (TRMSE) within t∈[0,5s] were calculated. RMSE and TRMSE are commonly used indicators for evaluating the accuracy of state estimation. Taking the position error in the x-axis direction as an example, the RMSE and TRMSE are calculated as follows:

[0271]

[0272] Among them, M = 100 is the number of Monte Carlo simulations, and N = 500 is the total number of sampling points. is the estimated value of the x-axis position of the i-th Monte Carlo simulation at time k, x k is the true value of the x-axis position at time k. The TRMSE within the sampling time is shown in Tables 2 to 5.

[0273] Table 2 TRMSE of position estimates

[0274]

[0275] Table 3 TRMSE of velocity estimates

[0276]

[0277] Table 4 TRMSE of pose estimation

[0278]

[0279] Table 5 TRMSE of angular velocity estimates

[0280]

[0281] The first three rows in the table are the state estimation accuracy in the x, y, and z directions respectively, and the last row is the total estimation accuracy.

[0282] The following conclusions can be drawn from the simulation results:

[0283] (1) The m sensors are distributed around the target trajectory, indicating good geometric conditions. However, the MS method has large estimation errors and high uncertainty. This is because the system state dimension is high relative to the measurement information. The QEKF method, however, adds model constraints, which improves the state estimation accuracy.

[0284] (2) Compared with QEKF, InEKF uses mixed state variables, part of the state is located in the Lie group space, and the error covariance matrix is ​​propagated in the Lie algebra. However, the error transfer matrix and the measurement matrix are still related to the current state estimation error and do not satisfy the invariance. Therefore, the state estimation accuracy of multi-beacon targets is not significantly improved.

[0285] (3) In LUKF-LG, ALUKF-LG and ARUKF-LG, the state is in the SE5(3) space. The posterior probability distribution of the state is approximated by sigma and UT transformation in Lie algebra, which improves the consistency of the state space and avoids the linearization process. Therefore, its estimation accuracy is higher than that of the QEKF and LInEKF methods.

[0286] (4) ALUKF-LG and ARUKF-LG can obtain more accurate multi-beacon target positions, velocities, rotation Euler angles, and angular velocities. In addition to the state variables based on the Lie group space, this is also due to the adaptive SNCM and MNCM estimation methods. Therefore, they are robust to complex positioning environments and inaccurate prior information. ALUKF-LG is relatively better than ARUKF-LG, which shows that the definition of the state error in the Lie group space will affect the positioning results.

[0287] In summary, in a multi-beacon target positioning system based on distance and radial velocity measurements, the proposed adaptive Lie group unscented Kalman filtering method can improve the state space consistency and the robustness to inaccurate noise covariance, accurately correct the tracking position inconsistency error, and significantly improve the estimation accuracy of the position, velocity, rotation Euler angle, and angular velocity of multi-beacon targets.

[0288] The above description of the disclosed embodiments is intended to enable any person skilled in the art to implement or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be applied to other embodiments without departing from the spirit and scope of the present disclosure. Therefore, the present disclosure is not limited to the embodiments presented herein but is intended to be consistent with the broadest scope of the principles and novel features disclosed herein.

[0289] The specific implementation methods described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific implementation method of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for determining the motion state of an aircraft based on Lie group unscented Kalman filtering, characterized in that: include: Establishing a state model of a target aircraft and a measurement model of the target aircraft; Obtaining a final Lie group state estimate of the target aircraft through iterative calculation according to the state model and the measurement model; The motion state of the target aircraft is determined according to the final Lie group state estimation value.

2. The method for determining the motion state of an aircraft based on Lie group unscented Kalman filtering according to claim 1, wherein: The state model is of the form: x k =f χ (x k-1 ,he k-1 ), Where χ represents the estimated value of the Lie group state, k represents the current moment, k-1 represents the previous moment, ε is the Gaussian white noise in the Euclidean space, and Q represents the state noise covariance matrix, 0 q represents the q-dimensional zero vector; The measurement model is of the form: z k =h χ (x k )+υ k Wherein, z represents the measurement information of the target aircraft, R represents the measurement noise covariance matrix, υ represents the measurement noise, and m represents the number of sensors used to measure the target aircraft.

3. The method for determining the motion state of an aircraft based on Lie group unscented Kalman filtering according to claim 2, wherein: The step of obtaining a final Lie group state estimate of the target aircraft through iterative calculation based on the state model and the measurement model includes: Determining an initial value of a Lie group state prediction value, an initial value of a state noise covariance matrix, and an initial value of a measurement noise covariance matrix based on prior information, wherein the Lie group state includes motion state information of the target aircraft; Set the initial value of the number of iterations k to k = 1; Using the initial value of the Lie group state prediction value as the Lie group state prediction value at time k-1, using the initial value of the state noise covariance matrix as the state noise covariance matrix at time k-1, and using the initial value of the measurement noise covariance matrix as the measurement noise covariance matrix at time k-1; Obtaining a Lie group state prediction value at time k according to the Lie group state prediction value at time k-1 and the state noise covariance matrix at time k-1; Obtaining a Lie group state estimate at time k based on the Lie group state prediction value at time k and the measurement noise covariance matrix at time k-1; Determine the k-time state noise covariance matrix and the k-time measurement noise covariance matrix according to the k-time Lie group state prediction value and the k-time Lie group state estimation value; Update the number of iterations k = k + 1; When k < k max When , the above iterative calculation process is repeatedly performed, and the initial step of the iterative calculation process is to obtain the Lie group state prediction value at time k according to the Lie group state prediction value at time k-1 and the state noise covariance matrix at time k-1; The last obtained Lie group state estimation value at time k is used as the final Lie group state estimation value.

4. The method for determining the motion state of an aircraft based on Lie group unscented Kalman filtering according to claim 3, wherein: The step of obtaining the Lie group state prediction value at time k according to the Lie group state prediction value at time k-1 and the state noise covariance matrix at time k-1 specifically includes: A one-step prediction is performed based on the k-1 moment Lie group state prediction value, the k-1 moment state noise covariance matrix and the state model to obtain the k- moment Lie group state prediction value.

5. The method for determining the motion state of an aircraft based on a Lie group unscented Kalman filter according to claim 4, further comprising, after obtaining the Lie group state prediction value at time k: Determining a first scaling parameter by using a preset state dimension, a scaling coefficient, and a noise dimension; Determining the weight of a first augmented sigma point using the first scaling parameter; Obtaining a first augmented covariance matrix according to the k-1 time Lie group state prediction value and the k-1 time state noise covariance matrix, and determining the first augmented sigma point through the first augmented covariance matrix; Splitting the first augmented sigma point to obtain a first Lie algebraic error sigma point and a state noise sigma point; Calculating a Lie group state error based on the Lie group state prediction value at time k, the first Lie algebraic error sigma point, and the state noise sigma point; According to the weight of the first augmented sigma point and the Lie group state error, a predicted value of the Lie algebraic error covariance matrix at time k is calculated by adopting a logarithmic mapping and then weighting method.

6. The method for determining the motion state of an aircraft based on Lie group unscented Kalman filtering according to claim 5, wherein: Obtaining the estimated value of the Lie group state at time k according to the predicted value of the Lie group state at time k and the measurement noise covariance matrix at time k-1 specifically includes: determining a second scaling parameter by using the state dimension and a preset measurement dimension; Determining the weight of the second augmented sigma point by using the second scaling parameter; Obtaining a second augmented covariance matrix according to the Lie group state prediction value at time k and the measurement noise covariance matrix at time k-1, and determining the second augmented sigma point through the second augmented covariance matrix; Splitting the second augmented sigma point to obtain a second Lie algebraic error sigma point and a measurement noise sigma point; Determining a measurement prediction value of the second augmented sigma point according to the Lie group state prediction value at time k, the second Lie algebraic error sigma point, the measurement noise sigma point, and the measurement model; Averaging the second augmented sigma points to obtain a second augmented sigma point mean vector, and then performing weighted summation on the measurement prediction values ​​using the weights of the second augmented sigma points to obtain a measurement prediction mean, an error covariance of the measurement prediction values, and an error covariance between the second augmented sigma point mean vector and the measurement prediction values; Obtaining a Lie algebra error estimate at time k based on the measurement prediction mean, the error covariance of the measurement prediction value, the error covariance between the second augmented sigma point mean vector and the measurement prediction value, and measurement information; The Lie group state estimation value at the k moment is obtained according to the Lie group state prediction value at the k moment and the Lie algebra error estimation value at the k moment.

7. The method for determining the motion state of an aircraft based on Lie group unscented Kalman filtering according to claim 6, wherein: After obtaining the estimated value of the Lie group state at time k, the method further includes: According to the Lie algebraic error estimation value at time k and the measurement noise covariance matrix at time k-1, the predicted value of the Lie algebraic error covariance matrix at time k is updated to obtain the Lie algebraic error covariance matrix at time k.

8. The method for determining the motion state of an aircraft based on Lie group unscented Kalman filtering according to claim 7, wherein: The step of determining the k-time state noise covariance matrix and the k-time measurement noise covariance matrix according to the k-time Lie group state prediction value and the k-time Lie group state estimation value respectively specifically includes: Converting the k-time Lie group state prediction value into the k-time Euclidean space state prediction value, and converting the k-time Lie group state estimation value into the k-time Euclidean space state estimation value; Using Kalman filtering to calculate the Euclidean space state prediction value at time k and the Euclidean space state estimation value at time k, to obtain an error covariance matrix and a gain matrix about the Euclidean space state prediction value at time k; Calculating the measurement residual of the Euclidean space state estimate at the k-th moment; The k-1 moment measurement noise covariance matrix is ​​updated according to the measurement residual and the error covariance matrix of the Euclidean space state prediction value at the k moment, thereby obtaining the k moment measurement noise covariance matrix; The state noise covariance matrix at time k-1 is updated according to the gain matrix, the measurement information and the Euclidean space state prediction value at time k, thereby obtaining the state noise covariance matrix at time k.

Images