Multi-node distributed filtering estimation method under flexible connection constraint

Abstract

The application discloses a kind of flexible connection constraint under multi-node distributed filtering estimation method, belong to deep space exploration technical field.The implementation method of the application is as follows: by selecting the node with maximum observability in adjacent node, the state estimation thereof is utilized to establish flexible connection constraint, and the constraint least square problem of distributed filtering estimation is formed;The condition that inequality constraint is converted into equality constraint needs to be satisfied is analyzed using KKT condition, the flexible connection constraint is converted from inequality constraint to equality constraint, to avoid optimization process under the influence of inequality constraint to fall into local optimum, by constraint local linearization, the analytical expression of node state estimation mean value and estimation error covariance is obtained, the efficient estimation of flexible lander node state is realized, and the state estimation result conforming to the physical characteristics of flexible connection is generated;In addition, the nonlinear constraint linearization error is reduced by iterative estimation, and the node state estimation accuracy of flexible lander is further improved.

Description

Technical Field

[0001] This invention relates to a multi-node distributed filtering estimation method under flexible connection constraints, belonging to the field of deep space exploration technology. Background Technology

[0002] Considering that traditional rigid landers are prone to overturning and bouncing when they come into contact with the surface of small celestial bodies, resulting in strong uncertainty in the attachment mission, some scholars have proposed to use a flexible lander with a planar shape to attach to the surface of small celestial bodies. By increasing the contact area between the lander and the small celestial body, overturning can be avoided, and the residual kinetic energy after the collision can be consumed by flexible materials to avoid bouncing. This provides a new solution for achieving highly reliable and highly adaptable landing on the surface of small celestial bodies.

[0003] Flexible landers are infinite-dimensional distributed parameter systems. To facilitate the design of navigation and control methods, a simplified three-node configuration is often used as a model for flexible landers in research. The nodes are connected by flexible materials, and the relative distances vary within a given range. Mathematically, this is a nonlinear inequality constraint incorporating the states of two nodes. Introducing flexible connection constraints into the flexible attachment state estimation problem can eliminate large-biased estimation results that do not satisfy the constraints, thus improving the convergence speed of the filtered estimation.

[0004] Existing methods, based on the obtained node state estimates, correct the estimated states of the three nodes by solving a least-squares problem with flexible connection constraints, employing a centralized collaborative correction approach. However, this method can only correct the mean of the node state estimates, failing to correct the estimation error covariance. Furthermore, since the flexible connection constraint is a non-convex constraint, solving it using conventional nonlinear programming methods is prone to getting trapped in local optima. Summary of the Invention

[0005] The main objective of this invention is to provide a multi-node distributed filtering estimation method under flexible connection constraints. By selecting the node with the highest observability among its neighboring nodes and using its estimated state to establish flexible connection constraints, a constrained least squares problem for distributed filtering estimation is formed. The method utilizes KKT (Karush–Kuhn–Tucker) condition analysis to transform inequality constraints into equality constraints, thus converting the flexible connection constraints from inequality constraints to equality constraints. This avoids the optimization process from getting trapped in local optima under the influence of inequality constraints. Through constraint local linearization, analytical expressions for the mean of node state estimates and the covariance of estimation errors are obtained, achieving efficient estimation of the flexible lander's node states and generating state estimation results that conform to the physical characteristics of flexible connections. Furthermore, this invention reduces the linearization error of nonlinear constraints through iterative estimation, further improving the accuracy of flexible lander node state estimation.

[0006] The objective of this invention is achieved through the following technical solution.

[0007] The multi-node distributed filtering estimation method under flexible connection constraints disclosed in this invention includes the following steps:

[0008] Step 1: To address the need for multi-node state estimation under flexible connection constraints, the node with the highest observability among the adjacent nodes is selected, and its estimated state is used to establish flexible connection constraints, forming a constrained least squares problem for distributed filtering estimation.

[0009] The flexible lander is equipped with M sensor nodes. The state equation of node i (i1,...,M) is as follows:

[0010]

[0011] Where x i,t Let x be the state of node i at time t. i,t-1 Let u be the state of node i at time t-1. i,t f is the control input at node i at time t. i (·) is the state transition equation, q i,t This represents system noise. The observation equation is:

[0012] z i,t h i (x i,t )+w i,t (2)

[0013] Where z i,t Let h be the observation at node i at time t. i (·) represents the observation equation, w i,t To detect noise, the estimated state of node i at time t is obtained using a nonlinear filtering method based on formulas (1) and (2). and estimation error covariance P i,t .

[0014] Preferably, the nonlinear filtering method includes extended Kalman filtering, unscented Kalman filtering, and particle filtering.

[0015] Under the action of flexible connection, the relative distance between nodes varies within a given range, and the node position vectors satisfy the constraints.

[0016]

[0017] Where r i,t ,r j,t Let be the position vectors of node i and node j at time t, respectively. Let be the minimum and maximum relative distances between nodes i and j, respectively, and ||·|| denote the magnitude of the vector.

[0018] Let the set of neighboring nodes of node i be . For nodes Its Fisher information matrix at time t is

[0019]

[0020] Where m represents the number of observations at node j. Let σ be the κ-th observation equation for node j. j,κ Let x be the standard deviation of the observation error of the κ-th observation equation. j,t The state of node j at time t.

[0021] Define the observability of node j as the determinant of the Fisher information matrix, det(F j,t According to the Cramér-Rao inequality, the state estimation error covariance P of node j is... j,t satisfy

[0022]

[0023] Where tr(·) represents the trace of the matrix, λ p Fisher information matrix F j,t The eigenvalues ​​satisfy det(F) j,t )=λ1...λ n The greater the observability of node j, the greater the state estimation error covariance P. j,t The smaller the lower bound, the higher the accuracy of the state estimation for node j.

[0024] In the set of neighboring nodes of node i Select the node j* with the maximum observability, i.e. The estimated state of node j* Substituting into formula (3), we obtain the flexible connection constraint.

[0025]

[0026] in Let $\mathbf$ be the minimum and maximum relative distances between nodes $i$ and $j$, respectively. Estimate the state at node $i$. Based on this, a constrained least squares problem for distributed filter estimation is established.

[0027]

[0028] in To estimate the position of node i, W i,t Weight matrix.

[0029] Step 2: Based on the constrained least squares problem of distributed filtering estimation established in Step 1, the conditions that need to be met to transform inequality constraints into equality constraints are analyzed using KKT conditions. The flexible connection constraints are transformed from inequality constraints to equality constraints to avoid the optimization process getting trapped in local optima under the influence of inequality constraints. Through constraint local linearization, the analytical expressions of the mean of node state estimation and the covariance of estimation error are obtained, realizing efficient estimation of the node state of the flexible lander and generating state estimation results that conform to the physical characteristics of the flexible connection.

[0030] The Lagrangian function for the constrained least squares problem established in step one is:

[0031]

[0032] Where μ1 and μ2 are Lagrange multipliers. According to the KKT conditions, the optimal solution... Need to meet

[0033]

[0034] μ1≥0 (10)

[0035]

[0036] μ2≥0 (12)

[0037]

[0038]

[0039] The value can be divided into three cases:

[0040] ①When and At that time, according to formulas (9) to (13) we have

[0041]

[0042] Then the optimal solution At the same time, according to formula (14), we have

[0043]

[0044] ②When At that time, according to formulas (9), (12) to (13) we have

[0045] μ2=0 (17)

[0046]

[0047] but

[0048]

[0049] When the weight matrix W i,t When =wI, w is a positive real number and I is the identity matrix. Substituting formula (19) into get

[0050]

[0051] because Therefore, there is

[0052]

[0053] ③When At that time, according to formulas (9), (10) to (11) we have

[0054] μ1=0 (22)

[0055]

[0056] but

[0057]

[0058] When the weight matrix W i,t When wI, ​​substitute formula (24) into get

[0059]

[0060] because Therefore, there is

[0061]

[0062] Based on situations ①②③, the flexible connection constraint (6) is processed, and the conditions that need to be met for the flexible connection constraint to be transformed from an inequality constraint to an equality constraint are analyzed:

[0063] When node i estimates its position The optimal solution that minimizes the performance index J of the constrained least squares problem (7) when formula (16) is satisfied. The mean of the node state estimate and the covariance of the estimation error remain unchanged.

[0064] When node i estimates its position When formula (21) is satisfied, if but The value of satisfies condition ① or ③. Based on condition ①, we obtain... This contradicts formula (21); based on case ③, we obtain... This contradicts formula (21). Therefore, when node i estimates its position... When formula (21) is satisfied, the optimal solution satisfies This transforms the flexible connection constraint (6) from an inequality constraint into an equality constraint.

[0065] When node i estimates its position If formula (26) is satisfied, but The value of satisfies condition ① or ②. Based on condition ①, we obtain... This contradicts formula (26); according to case ②, we get This contradicts formula (21). Therefore, when node i estimates its position... When formula (26) is satisfied, the optimal solution satisfies This transforms the flexible connection constraint (6) from an inequality constraint into an equality constraint.

[0066] When node i estimates its position When formula (21) or formula (26) is satisfied, the flexible connection constraint (6) is transformed from an inequality constraint into an equality constraint of the following form.

[0067]

[0068] In formula (27), when Time variable when Time variable exist The nonlinear constraint (27) is locally linearized in the vicinity, and higher-order terms are ignored:

[0069]

[0070] Where D is exist Let be the Jacobian matrix at . Then the location-related components in the mean of the node state estimate and the covariance of the estimation error are updated as follows:

[0071]

[0072]

[0073] We obtain analytical expressions for the mean of the node state estimates and the covariance of the estimation error.

[0074] Using the KKT conditions, the conditions (21) and (26) that need to be satisfied to transform inequality constraints into equality constraints are analyzed. The flexible connection constraints are transformed from inequality constraints into equality constraints, avoiding getting trapped in local optima when solving the constraint least squares problem. By linearizing the constraints locally, the analytical expressions of the mean of the node state estimate and the covariance of the estimate error are obtained, realizing efficient estimation of the node state of the flexible lander and generating state estimation results that conform to the physical characteristics of the flexible connection.

[0075] To further improve the accuracy of node state estimation for flexible landers, step three is also included: based on the analytical expressions of the node state estimation mean and estimation error covariance obtained in step two, the node state estimation mean and estimation error covariance are updated using an iterative estimation method to reduce the linearization error of nonlinear constraints and further improve the accuracy of node state estimation for flexible landers.

[0076] In step two, formula (28) is used in... By locally linearizing the nonlinear constraint (27) in the vicinity, analytical expressions for the mean of the nodal state estimate and the covariance of the estimate error are obtained. and When the gap is large, constraint linearization will introduce significant errors and affect the accuracy of node state estimation.

[0077] An iterative estimation method is used to update the mean and covariance of the node state estimates, reducing the impact of constraint linearization error. Equations (28) to (30) are improved as follows:

[0078]

[0079]

[0080]

[0081] in D represents the k-th iteration result of the position-related components in the mean and covariance of the estimation error of node i, respectively. (k 1) for exist Jacobian matrix at the location, For the first iteration,

[0082] The mean and covariance of the estimated node states are updated using formulas (31) to (33) until... Where ε is a given upper bound on the error, and let

[0083] This reduces the linearization error of nonlinear constraints through iterative estimation, thereby further improving the accuracy of node state estimation for flexible landers.

[0084] It also includes step four, which involves using the node state estimation results of the flexible lander obtained in step two or three to achieve autonomous navigation, real-time trajectory planning and guidance control of the flexible lander.

[0085] Beneficial effects:

[0086] 1. The multi-node distributed filtering estimation method under flexible connection constraints disclosed in this invention selects the node with the highest observability among neighboring nodes and uses its estimated state to establish flexible connection constraints, forming a constrained least squares problem for distributed filtering estimation. By using KKT condition analysis to transform inequality constraints into equality constraints, the flexible connection constraints are transformed from inequality constraints to equality constraints. Through constraint local linearization, analytical expressions for the mean of node state estimation and the covariance of estimation error are obtained, realizing efficient estimation of the node state of the flexible lander and generating state estimation results that conform to the physical characteristics of flexible connection.

[0087] 2. The multi-node distributed filtering estimation method under flexible connection constraints disclosed in this invention selects the node with the highest observability among the adjacent nodes and uses its estimated state to establish flexible connection constraints, forming a constrained least squares problem for distributed filtering estimation. This reduces the dimensionality of the optimization variables in the constrained least squares problem, improves the solution efficiency of the optimization problem, and enhances the real-time performance of multi-node distributed filtering estimation for flexible landers under the condition of limited onboard computing resources.

[0088] 3. The multi-node distributed filtering estimation method under flexible connection constraints disclosed in this invention utilizes the KKT conditions to process the flexible connection constraints (6) according to three cases, transforming the flexible connection constraints from inequality constraints to equality constraints, avoiding the optimization process from getting trapped in local optima under the influence of inequality constraints. Through constraint local linearization, the analytical expressions of the node state estimation mean and estimation error covariance are obtained, realizing efficient estimation of the node state of the flexible lander and generating state estimation results that conform to the physical characteristics of flexible connection.

[0089] 4. The multi-node distributed filtering estimation method under flexible connection constraints disclosed in this invention uses multiple iterative estimations to update the mean of node state estimation and the covariance of estimation error, thereby reducing the linearization error of nonlinear constraints and further improving the node state estimation accuracy of flexible landers. Attached Figure Description

[0090] Figure 1 This is a flowchart of the multi-node distributed filtering estimation method under flexible connection constraints disclosed in this invention.

[0091] Figure 2The state estimation error curve for node 1 of the flexible lander;

[0092] Figure 3 The state estimation error curve for node 2 of the flexible lander;

[0093] Figure 4 The state estimation error curve for node 3 of the flexible lander;

[0094] Figure 5 The constraint satisfaction condition for the flexible connection of node 2;

[0095] Figure 6 The constraint satisfaction condition for the flexible connection of node 3;

[0096] Figure 7 The state estimation error curve for node 2 after multiple iterations;

[0097] Figure 8 The curve shows the state estimation error of node 3 after multiple iterations. Detailed Implementation

[0098] To better illustrate the purpose and advantages of the present invention, the invention will be further described below in conjunction with the accompanying drawings and examples.

[0099] Example 1:

[0100] Simulation of a flexible attachment scenario on a small celestial body (433Eros) based on its gravitational field. Nodal dynamics equations are established under a fixed-connection system at the landing site.

[0101]

[0102] Where r i ,v i The positions and velocities of node i and u are respectively. i For node i, u i,F For the acceleration of the flexible connection, g(r) i Let q be the gravitational acceleration. i For system noise, satisfy q i ~N(0,1e -4 ). Initial position r of node 1 10 =[-9,25,10,300] T m, target landing point position r 1f =[0.75,0,10] T m, initial position of node 2 r 20 =[-10.38,10.65,300] T m, target landing point position r 2f =[-0.38, 0.65, 10] T m, initial position of node 3 r30 =[-10.38,9.35,300] T m, target landing point position r 3f = [-0.38, -0.65, 10] T m. All nodes have the same initial and final velocities, v. 10 =v 20 =v 30 [0,0,-0.2] T m / s, v 1f =v 2f =v 3f =[0,0,0] T m / s.

[0103] like Figure 1 As shown in the figure, the multi-node distributed filtering estimation method under flexible connection constraints disclosed in this embodiment has the following specific implementation steps:

[0104] Step 1: To address the need for multi-node state estimation under flexible connection constraints, the node with the highest observability among the adjacent nodes is selected, and its estimated state is used to establish flexible connection constraints, forming a constrained least squares problem for distributed filtering estimation.

[0105] The flexible lander adopts a simplified three-node model, with an optical camera installed at each node and a laser rangefinder installed at node 3. The optical camera has a focal length of f = 0.02m and a field of view of 40°. The observation noise follows a zero-mean Gaussian distribution, with a standard deviation of 2 pixels for camera observation noise and a standard deviation of 0.1m for laser rangefinder measurement noise.

[0106] For the three nodes, when a navigation landmark with a known absolute position is observed within the field of view of the node's optical camera, an observation equation is established.

[0107]

[0108] Where [u L ,v L ] T The coordinates of navigation landmarks in pixel format. For the position vector of the navigation landmark in the nodal camera's own system, satisfying

[0109]

[0110] Where r L To determine the position of the navigation landmark under the fixed connection of the attachment point, r i Let be the position of node i under the fixed connection at the attachment point. Let be the rotation matrix of the system that is fixedly connected to node i at the attachment point.

[0111] For node 3, the laser rangefinder can measure the distance from the node to a specified point (x) on the surface of the small celestial body. P ,y P ,z P distance The observation equation is

[0112]

[0113] Where x3, y3, z3 are the three-axis components of the position vector r3 of node 3.

[0114] During the attachment process, node 1 can observe 3 navigation landmarks, node 2 can observe 2 navigation landmarks, and node 3 can observe 1 navigation landmark and acquire measurement information from the laser rangefinder. An extended Kalman filter is used to obtain the estimated state of node i at time t. and estimation error covariance P i,t The result is as follows Figures 2 to 4 As shown. Figure 2 In the diagram, the solid line represents the position estimation error of node 1, and the dotted line represents the 3σ error bound of the position estimation. Figure 3 and Figure 4 In the diagram, the dashed lines represent the position estimation errors of node 2 and node 3, respectively.

[0115] The nominal distance between nodes is 1.3m. Under the action of flexible connection, the relative distance between nodes varies within the range of 80%-120% of the nominal distance. The upper and lower bounds of the flexible connection constraint are respectively...

[0116] For nodes 2 and 3, the node with the highest observability among their neighboring nodes is node 1, i.e., j* = 1. The estimated state of node 1 is... Substituting into formula (3), we obtain the flexible connection constraint, thus forming the constrained least squares problem of distributed filter estimation.

[0117]

[0118] Where i = 2, 3, and the weight matrix W i,t =I.

[0119] Step 2: Based on the constrained least squares problem of distributed filtering estimation established in Step 1, the conditions that need to be met to transform inequality constraints into equality constraints are analyzed using KKT conditions. The flexible connection constraints are transformed from inequality constraints to equality constraints to avoid the optimization process getting trapped in local optima under the influence of inequality constraints. Through constraint local linearization, the analytical expressions of the mean of node state estimation and the covariance of estimation error are obtained, realizing efficient estimation of the node state of the flexible lander and generating state estimation results that conform to the physical characteristics of the flexible connection.

[0120] For nodes 2 and 3, according to and The value of determines whether the optimal solution of the constrained least squares problem (38) lies inside the flexible connection constraint or on the boundary. The result is as follows: Figures 5 to 6 As shown. Figure 5 Let's consider the constraint satisfaction condition for node 2. Figure 6 For node 3, the constraint is satisfied, where "×" indicates that the optimal solution is inside the flexible connection constraint, and the mean and covariance of the node state estimate remain unchanged; "○" indicates that the optimal solution satisfies the upper boundary of the flexible connection constraint, and "△" indicates that the optimal solution satisfies the lower boundary of the flexible connection constraint. The flexible connection constraint (6) is transformed from an inequality constraint to an equality constraint, and the position-related components in the mean and covariance of the node state estimate are updated according to formulas (29) and (30). The node state estimation results are as follows: Figure 3 and Figure 4 As shown by the solid line, Figure 3 The solid line in the middle represents the position estimation error of node 2 after considering the flexible connection constraint. Figure 4 The solid line represents the position estimation error of node 3 after considering the flexible connection constraint. After introducing the flexible connection constraint, the position estimation accuracy of both nodes 2 and 3 was improved, except for the Z-axis position component of node 3.

[0121] Step 3: Based on the analytical expressions of the node state estimation mean and estimation error covariance obtained in Step 2, the node state estimation mean and estimation error covariance are updated by iterative estimation to reduce the linearization error of nonlinear constraints and further improve the node state estimation accuracy of the flexible lander.

[0122] To further improve the accuracy of the flexible lander node state estimation, the mean of the state estimation and the covariance of the estimation error of node 2 and node 3 are iteratively updated using formulas (31) to (33) until... The state estimation results are as follows Figure 7 and Figure 8 As shown, after iterative estimation, more accurate estimated states for nodes 2 and 3 are obtained. Throughout the flexible attachment process, the root mean square errors of position estimation for node 2 after 0 iterations and 5 iterations are 5.6521m and 5.2542m, respectively, while the root mean square errors of position estimation for node 3 after 0 iterations and 5 iterations are 3.7228m and 3.6970m, respectively.

[0123] Step 4: Based on the node state estimation results of the flexible lander obtained in Step 2 or Step 3, realize the autonomous navigation, real-time trajectory planning and guidance control of the flexible lander.

[0124] The above detailed description further illustrates the purpose, technical solution, and beneficial effects of the invention. It should be understood that the above description is only a specific embodiment 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 within the scope of protection of the present invention.

Claims

1. A method for multi-node distributed filtering estimation under flexible connection constraints, characterized in that: Comprising the following steps, Step one, for the state estimation demand of multiple nodes under the constraint of flexible connection, by selecting the node with the maximum degree of observation in the adjacent nodes, the state estimation of the node is used to establish the constraint of flexible connection, and a constrained least square problem of distributed filtering estimation is formed; A flexible lander is mounted with a sensor node; the state equation of the node is where is the state of the node at time , is the state of the node at time , is the control input to the node at time , is the state transition equation, is the system noise; and the observation equation is wherein is the time node the observation, is the observation equation, is the observation noise; according to formula (1) and formula (2), a nonlinear filtering method is used to obtain the time node the estimated state and the estimated error covariance ; Under the action of flexible connection, the relative distance between nodes changes within a given range, and the node position vector satisfies the constraint in They are nodes and nodes exist Position vector at time, They are nodes and nodes Minimum and maximum relative distance, Represents the magnitude of a vector; Let the set of neighboring nodes of node be , and for node , the Fisher information matrix at time is where denotes a node number of observations, is a node the th observation equation, is the th observation equation, and node state at time instance Definition node The observability of the definition node is the determinant of the Fisher information matrix ; According to the Cramér-Rao inequality, the state estimation error covariance of the nodes satisfies where denotes the trace of a matrix, is the eigenvalue of Fisher information matrix satisfying The larger the observability of node , the smaller the lower bound of state estimation error covariance , and the higher the state estimation accuracy of node . In the node The node with the largest observability is selected from the adjacent node set of the node , that is , the estimated state of the node is substituted into formula (3) to obtain the flexible connection constraint where are the nodes and the nodes are the minimum and maximum relative distances; at the nodes the estimated states a constrained least squares problem for the distributed filtering estimation is established wherein is a node estimating a position, a weight matrix; Step two, based on the constrained least square problem of distributed filtering estimation established in step one, the conditions required for transforming the inequality constraint into an equality constraint are analyzed by using the KKT condition, the flexible connection constraint is transformed from an inequality constraint into an equality constraint, the optimization process is avoided from falling into a local optimum under the influence of the inequality constraint, the analytical expression of the node state estimation mean value and estimation error covariance is obtained through local linearization of the constraint, efficient estimation of the node state of the flexible lander is realized, and a state estimation result conforming to the physical characteristics of the flexible connection is generated.

2. The method of claim 1, wherein: Step three, based on the analytical expression of the node state estimation mean value and estimation error covariance obtained in step two, the node state estimation mean value and estimation error covariance are updated in an iterative estimation manner, the linearization error of the nonlinear constraint is reduced, and the node state estimation accuracy of the flexible lander is further improved.

3. The method of claim 2, wherein: Step four, according to the node state estimation result of the flexible lander obtained in step two or step three, autonomous navigation, real-time trajectory planning and guidance control of the flexible lander are realized.

4. The method of claim 1, wherein: The method for realizing step two is that the Lagrange function of the constrained least square problem established in step one is where is the Lagrange multiplier; according to the KKT conditions, the optimal solution must be satisfied The value of is divided into three cases: When and then according to equations (9) to (13) Then the optimal solution and according to equation (14) When , according to the formula (9), the formula (12) to (13) have then When the weight matrix is positive real, is a positive real number, is the identity matrix, substituting equation (19) into yields As a result therefore When , according to the formula (9), the formula (10) to (11) have Then When the weight matrix is substituted into equation (24) we obtain As a result therefore According to the cases ①②③, the flexible connection constraint (6) is processed, and the conditions that the flexible connection constraint needs to meet when the inequality constraint is converted into the equality constraint are analyzed. When the node Estimate position Satisfy formula (16), so that the performance index of the constraint least squares problem (7) Optimal solution that takes the minimum value , the node state estimation mean and estimation error covariance remain unchanged; When the node Estimate position If , the value of satisfies case ① or ③; according to case ①, it is obtained that , which is contradictory to formula (21); according to case ③, it is obtained that , which is contradictory to formula (21); Thus, when the node estimating the position The optimal solution satisfies Thus, the flexible connection constraint (6) is converted from an inequality constraint to an equality constraint; When the node estimates the position , if , then the value of satisfies case ① or ②; according to case ①, it is obtained that , which is contradictory to formula (26); according to case ②, it is obtained that , which is contradictory to formula (21); Thus, when the node estimating the position The optimal solution satisfies Thus, the flexible connection constraint (6) is converted from an inequality constraint to an equality constraint; When the node Estimate position When the formula (21) or the formula (26) is satisfied, the flexible connection constraint (6) is converted from an inequality constraint to an equality constraint in the following form In formula (27), when the variable ; when the variable ; near the nonlinear constraint (27) is locally linearized and the high-order terms are ignored: where is At the Jacobian matrix; let then the components of the node state estimation mean and estimation error covariance related to the position are updated as obtain an analytical expression for the node state estimate mean and estimate error covariance; By using the KKT condition, the conditions (21) and (26) required for transforming the inequality constraint into an equality constraint are analyzed, the flexible connection constraint is transformed from an inequality constraint into an equality constraint, the solution of the constrained least square problem is avoided from falling into a local optimal solution, the analytical expression of the node state estimation mean value and estimation error covariance is obtained through local linearization of the constraint, efficient estimation of the node state of the flexible lander is realized, and a state estimation result conforming to the physical characteristics of the flexible connection is generated.

5. The method of claim 4, wherein: The method for realizing step three is that In step two, the non-linear constraint (27) is locally linearized around and the analytical expressions of the estimated mean and the estimated error covariance of the node state are obtained by formula (28); when and the difference is large, the constraint linearization will bring significant error, affecting the accuracy of the node state estimation. The node state estimation mean value and estimation error covariance are updated in an iterative estimation manner, the influence of the linearization error of the constraint is reduced, and formulas (28) to (30) are improved to in They are nodes The position-related components in the mean and covariance of the state estimate error are: The result of the second iteration for exist Jacobian matrix at the location, For the first iteration, ; The node state estimate mean and the estimate error covariance are updated using Equations (31) to (33) until where is a given error upper bound, and let , ; Thus, the nonlinear constraint linearization error is reduced through iterative estimation, and the node state estimation accuracy of the flexible lander is further improved.

6. The method of claim 4 or 5, wherein: The nonlinear filtering method includes extended Kalman filtering, unscented Kalman filtering and particle filtering.

Images