A local attention factor graph based geomagnetic / pdr indoor navigation method

Abstract

The application discloses a kind of indoor navigation methods of geomagnetic / PDR based on local attention factor graph.The method first collects data to construct geomagnetic fingerprint interface, estimates the measurement value of pedestrian step length and heading angle, and the measurement value of geomagnetic.Then construct local attention factor graph model, after the fusion of various measurement values, the navigation trajectory problem is converted into a constrained optimization problem.Finally, improved genetic algorithm is used to solve local attention factor graph model.Local attention factor graph converts navigation solving problem into a constrained optimization problem.Genetic algorithm is used to solve the optimal solution of optimization problem through the iterative mode of crossover, mutation, selection.The application converts complex navigation solving problem into a constrained optimization problem, ensures the rationality of solving result and improves the anti-interference ability of system;The coding and decoding model of genetic algorithm constructed in the application ensures the sustainability of iteration;The convergence speed of algorithm is improved.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of integrated navigation, and relates to a factor graph optimization and indoor positioning method, in particular to a geomagnetic / PDR indoor navigation method based on a local attention factor graph. BACKGROUND

[0002] In integrated navigation, the earth magnetic field is an important source of navigation information. However, the existing navigation methods have very limited use of geomagnetic information, and there are problems such as insufficient features, matching errors, and insufficient combination with other positioning sources, resulting in insufficient positioning result accuracy. The factor graph is a probabilistic graph model with the characteristics of "plug and play", and is widely used in the field of navigation and positioning. For example, the Chinese patent with the publication number CN115790592A uses the ISAM2 optimization algorithm to incrementally solve the state variables in the factor graph in the flow space, and compensates the earth rotation and harmful acceleration based on the current state and pre-integrated increment, thereby realizing the integrated navigation method based on the factor graph. However, the factor graph algorithm based on maximum a posteriori estimation has a single objective in the optimization stage, only focusing on maximizing or minimizing the objective function, and often lacks attention to the rationality of local factor optimization, which may lead to solutions that do not conform to reality, affecting the positioning accuracy. SUMMARY

[0003] In view of the deficiencies of the prior art, the application proposes a geomagnetic / PDR indoor navigation method based on a local attention factor graph, adds strict constraint conditions to the factor graph model to ensure the rationality of the solution and reduce the search space, uses the method of constructing a geomagnetic fingerprint library, combines the improved factor graph model with inertial navigation devices, uses an improved genetic algorithm to solve the factor graph model in the factor graph optimization stage, improves the navigation and positioning accuracy and ensures the rationality of the solution, and solves the problems of limited use of geomagnetic information and unreasonable solution of the factor graph in the prior art.

[0004] A geomagnetic / PDR indoor navigation method based on a local attention factor graph, specifically comprising the following steps:

[0005] Step 1: Collect magnetic field data in a grid in the navigation test area and perform time average calculation to construct a geomagnetic fingerprint library containing magnetic field size. Use the Bicubic interpolation method for interpolation to construct a geomagnetic fingerprint interface.

[0006] Step 2, continuously walking along the test trajectory in the navigation test area while collecting magnetic field data and inertial device data, including three-axis magnetometer measurements, three-axis accelerometer measurements, and three-axis gyroscope measurements. Use the step length estimation algorithm to calculate the single-step step length of walking as the step length measurement value; use the step monitoring algorithm to locate the step timestamp and calculate the heading angle of each step in the walking trajectory as the heading angle measurement value; calculate the geomagnetic magnitude at the corresponding timestamp as the geomagnetic measurement value.

[0007] Step 3, construct a local attention factor graph model, fuse the various measurement values obtained in step 2 through the local attention factor graph model, and convert the navigation trajectory problem into a constrained optimization problem, the specific steps are as follows:

[0008] Step 3.1, define the combined navigation system state transition model:

[0009] X i+1 =g(X i )+w i

[0010] Where X i =[x i ,y i ] T is the system state vector, x i and y i represent two coordinate values in the plane rectangular coordinate system respectively; g() is the state transition function, w i is the process noise, and i is the time sequence.

[0011] Define the measurement model of the combined navigation system:

[0012] Z i =h(X i )+v i

[0013] Where Z i is the measurement vector, h() is the measurement function, and v i is the measurement noise.

[0014] Step 3.2, construct the factor node model:

[0015]

[0016] Where f is the factor node function, L() is the error probability function, is the prediction of the system state vector.

[0017] Step 3.3, introduce local attention, constrain the feasible region range of the factor node, and reconstruct the factor graph model:

[0018]

[0019] where X MAP is the maximum a posteriori probability estimate of the state vector at all times, X = {X1, X2...X i ...X n}, ||·|| represents the solution of Mahalanobis distance, Σ is the residual covariance matrix, δ i is the constraint boundary.

[0020] Step 3.4, fuse the measurement values in step 2 using the local attention factor graph model:

[0021] Define a Gaussian probability distribution function to represent the error probability function L() in step 3.2, then the factor node model can be described as:

[0022]

[0023] Using the offline established magnetic fingerprint library and the magnetic interface in step 1, get the magnetic estimation value at any position in the region, and construct the magnetic factor:

[0024]

[0025] where f i mag is the magnetic factor, mag i is the magnetic measurement value, h mag () is the magnetic interface function, Σ mag is the magnetic error covariance matrix.

[0026] Rewrite the state transition model in step 3.1 into a measurement relationship, and describe the heading angle measurement θ i and the step length measurement steplen i as:

[0027]

[0028]

[0029] where h step () is the step length measurement function, is the step length measurement error; h angle () is the heading angle measurement function, is the heading angle measurement error. Further difference the heading angle at adjacent times to get the measurement model of the differential heading angle Δθ i :

[0030]

[0031] where h Dangle () is the differential heading angle measurement function, is the differential heading angle measurement error.

[0032] Assume that the step length measurement error at any two time instants is independent of each other, so the step length measurement error at different time is assumed to have the same probability distribution; assume that the bias error of the gyroscope at the same time has the same probability distribution, so the bias error of the gyroscope at different time is assumed to have the same probability distribution; therefore, the step length factor f i steplen and the differential heading angle factor f i Dangle are described as:

[0033]

[0034]

[0035] wherein Σ step is the step length error covariance matrix, and Σ Dangle is the differential heading angle error covariance matrix.

[0036] Step 3.5, the measurement error of the sensor in step 3.4 is assumed to satisfy the Gaussian probability distribution, that is, the probability of error is smaller when deviating from zero value is farther, adding local attention can limit the estimation of step length measurement error and differential heading angle error, without considering the error deviating too much. Therefore, the navigation solution problem is converted into a constrained optimization problem, which is described as:

[0037]

[0038] wherein · is the Euclidean distance function, is the maximum boundary of step length deviation, is the maximum boundary of differential heading angle deviation.

[0039] Since the magnetic field factor, the differential heading angle factor and the step length factor function are all in exponential form, the objective function can be equivalent to the least square form:

[0040]

[0041] Step 4, use the improved genetic algorithm to solve the local attention factor graph model, the specific steps are as follows:

[0042] Step 4.1, chromosome coding and decoding:

[0043] Define the length of state X as n, and the starting point state X0 and the starting heading angle θ0 are known. Define the coding mode of a single chromosome as:

[0044] C={c1,c2...c n ,c n+1 ...c2n}

[0045] where C is a chromosome vector consisting of 2n symbols, c k is the allele at the kth position; the first n alleles are step alleles, and the n+1th to 2nth alleles are differential heading angle alleles.

[0046] The mapping relationship is defined as follows:

[0047]

[0048] where c i and c i+n represent the alleles at the ith and i+nth positions, respectively. is the estimate of the step, is the step estimate of the differential heading angle; the value range of the allele c k is always limited to the interval (-1, 1), is the deviation of the step measurement from the estimate, is the deviation of the differential heading angle measurement from the estimate. Therefore, when the value range of the allele is always limited to the interval (-1, 1), the constraint condition in step 3.5 is always satisfied;

[0049] The heading angle backstepping model is defined as:

[0050]

[0051] where is the estimate of the heading angle, θ1 is the initial heading angle, and j represents the time sequence and j < i.

[0052] The state transition equation model is defined as:

[0053]

[0054] The above mapping relationship, heading angle backstepping model, and state transition equation model are collectively referred to as the decoding method of the chromosome. Through such a decoding method, a single chromosome is uniquely decoded into the estimates of all states.

[0055] Step 4.2, define the fitness calculation method:

[0056] The calculation method of the fitness of a single chromosome is defined as:

[0057]

[0058] where is equivalent to , is equivalent to Therefore, the fitness function can be further simplified as:

[0059]

[0060] The multi-segment fitness of a single chromosome is defined as:

[0061]

[0062] where fitness' is the fitness of the de segment, d and e are positive integers and satisfy 1≤d≤e≤2n; the multi-segment fitness is a standard for measuring the quality of a chromosome segment. d,e

[0063] Step 4.3, initialization of genetic algorithm:

[0064] Randomly generate 2n random real numbers in the range (-1, 1), and take these 2n random real numbers as one allele to form a chromosome. Set the maximum number of populations as groupsize, and consider each chromosome as an "individual". Then repeat groupsize times to obtain groupsize initialized chromosomes to form a population group, where the pth chromosome is described as C p .

[0065] Step 4.4, fitness calculation:

[0066] Select different d and e to divide the chromosome into several segments:

[0067]

[0068] where d w is the first allele index of the wth segment, e w is the last allele index of the wth segment, and w∈[1,mp]. Calculate the multi-segment fitness of each chromosome by segment, and retain the optimal chromosome segment:

[0069]

[0070] where D w is the optimal chromosome segment in the wth segment. Connect the heads and tails of all retained chromosome segments to form a new sequence B:

[0071] B={D1,D2...D mp-1 ,D mp}

[0072] Since each segment in the sequence comes from a chromosome, the new sequence B is still a chromosome. Empty the elite pool and put the new sequence B into the elite pool.

[0073] ​Step 4.5, chromosome crossover:

[0074] The fitness of each chromosome in the population group is calculated in turn. The iteration reservation number is set to parentnum, and selection is performed according to the value of the monomer chromosome fitness fitness. The smaller the fitness value, the greater the probability of being selected, and the chromosome with the smallest fitness is directly reserved. The selected chromosomes are used as parent chromosomes, and the unselected chromosomes are directly eliminated and do not participate in subsequent iterations.

[0075] Two different chromosomes C p1 and C p2 are randomly selected from the parent chromosomes.

[0076]

[0077]

[0078]

[0079] wherein is the lth allele on the pth chromosome; S k represents the new chromosome obtained by crossing, p cross () is the chromosome "crossing" function; g1 and g2 are randomly generated variables satisfying the relationship 1≤g1≤g2≤2n. S k all alleles come from parent chromosomes C p1 and C p2 , so each allele is in the interval (-1, 1).

[0080] The above crossing operation is repeated groupsize-parentnum-1 times to generate groupsize-parentnum-1 new chromosomes, and the newly generated chromosomes through the crossing operation are defined as child chromosomes. The parent and child chromosomes are combined into a new population group cross with a population size of groupsize-1.

[0081] Step 4.6, population mutation:

[0082] The mutation probability of the population group cross is set to Rate∈(0,1). A matrix with a row number of groupsize-1 and a column number of 2n is randomly generated, m row,col represents the element in the row row and the column col of the matrix, colm row,col ∈(0,1) and m row,col is a real number. For each m row,col , if m row,col<Rate then let the population group cross The col th allele of the row th chromosome Where randgenerate() is a random number generating function, outputting a random real number in the range (-1, 1). Traverse all m row,col , complete the mutation operation on the population group cross After mutation, each allele in the population is in the interval (-1, 1). Merge the population obtained after mutation with the sequence B in the elite pool to form a new population with groupsize individuals, and copy the values in the new population to group.

[0083] Step 4.7, iterative solution

[0084] Repeat steps 4.4-4.6, set the maximum number of iterations to generation, stop iteration when the number of repetitions is greater than generation, and maintain a complete population in each iteration algorithm. Select the chromosome with the minimum fitness in the population at this time as the output of the genetic algorithm solution: Use the decoding method defined in step 4.1 to decode the output of the genetic algorithm solution into a navigation solution:

[0085]

[0086] Where θ1 is the initial heading angle, X1 is the initial state of navigation, both are known quantities; steplen i is the step length measurement, Δθ i is the difference in heading angle measurement; is the final navigation solution after decoding.

[0087] The present application has the following beneficial effects:

[0088] 1. The present application introduces local attention into the factor graph-based integrated navigation problem, converts the complex navigation solution problem into a constrained optimization problem, and ensures the rationality of the solution result; when the sensor is abnormal, the local attention factor graph model-based integrated navigation problem solving limits the search space, and has the characteristics of not being easily affected by abnormalities, improving the reliability and accuracy of the system.

[0089] 2. The present application introduces genetic algorithm into the local attention factor graph-based solution, constructs a complete chromosome encoding and decoding model, ensures that the solution result always meets the constraint condition in the iteration process, and ensures the sustainability of the iteration; the search range under the chromosome encoding and decoding model completely contains the search space, ensuring the robustness of the navigation solution.

[0090] 3. The present application combines the definition of multi-section fitness and the elite strategy into the genetic algorithm optimization. According to the actual situation of the navigation positioning problem, some locally optimal chromosome fragments are retained, and the convergence speed of the algorithm is improved. BRIEF DESCRIPTION OF DRAWINGS

[0091] Figure 1 is a local attention factor graph-based magnetic / PDR indoor navigation method structure diagram;

[0092] Figure 2 is a local attention factor graph algorithm structure diagram;

[0093] Figure 3 is an improved genetic algorithm flowchart;

[0094] Figure 4 is an example test environment plan;

[0095] Figure 5 is a comparison diagram of the cumulative probability distribution of the navigation positioning solution error of various algorithms. DETAILED DESCRIPTION

[0096] The present application will be further explained in conjunction with the accompanying drawings;

[0097] The present application proposes a local attention factor graph-based magnetic / PDR indoor navigation method. Strict constraint conditions are added to the factor graph model to ensure the reasonableness of the solution and reduce the search space. The magnetic fingerprint library is constructed in combination with the improved factor graph model and inertial navigation device. In the factor graph optimization stage, the improved genetic algorithm is used to solve the factor graph model, which improves the navigation positioning accuracy and ensures the reasonableness of the solution, as shown in Figure 1 The specific steps include the following steps:

[0098] Step 1, in the test area as shown in Figure 2 , the magnetic field data is collected in grid order and time-averaged calculation to construct a magnetic fingerprint library containing the magnetic field size. The Bicubic interpolation method is used for interpolation to construct the magnetic fingerprint interface.

[0099] Step 2, continuously walk along the test trajectory in the navigation test area and collect magnetic field data and inertial device data at the same time, including three-axis magnetometer measurement, three-axis accelerometer measurement and three-axis gyroscope measurement. The step length estimation algorithm is used to calculate the single-step step length as the measured value of the step length; the step monitoring algorithm is used to locate the step timestamp, and the heading angle of each step of the trajectory is calculated as the measured value of the heading angle; the magnetic size at the corresponding timestamp is calculated as the measured value of the magnetic field.

[0100] Step 3, constructing a local attention factor graph model, fusing the various sensor measurements obtained in step 2 through the local attention factor graph model to obtain a model as shown in Figure 3 The model shown in figure 3 converts the navigation trajectory problem into a constrained optimization problem.

[0101] Step 4, the local attention factor graph converts the navigation problem into a constrained optimization problem, and the existing factor graph algorithm is invalid, and an improved genetic algorithm as shown in Figure 4 The improved genetic algorithm is used to solve the local attention factor graph model, and the optimal solution of the optimization problem is obtained through iteration of crossover, mutation and selection, and the final navigation solution is obtained by decoding.

[0102] The extended Kalman filter algorithm (EKF), the general factor graph algorithm (FG) and the present method are used respectively to solve the trajectory, and the solving trajectories obtained by the three methods are compared with the real trajectory, and the absolute error of the solving trajectory and the real trajectory at each time is calculated, and the cumulative probability distribution statistical analysis is carried out, and the results are shown in Figure 5 As shown in figure 3, the solving trajectory obtained by the present method is closest to the real trajectory.

[0103] The basic principles and main features of the present application and the advantages of the present application are shown and described. It should be understood by those skilled in the art that the present application is not limited by the above examples, and the above examples and descriptions in the specification are only to illustrate the principles of the present application, and various changes and improvements can be made to the present application without departing from the spirit and scope of the present application, and these changes and improvements all fall within the scope of the present application. The scope of protection of the present application is defined by the appended claims and their equivalents. The specific examples described herein are only illustrative of the spirit of the present application. Those skilled in the art can make various modifications or supplements to the described specific examples or use similar ways to replace them, but will not deviate from the spirit of the present application or exceed the scope defined by the appended claims.

Claims

1. A geomagnetic / PDR indoor navigation method based on local attention factor maps, characterized in that: Specifically, the following steps are included: Step 1: Collect magnetic field data and inertial device data of the navigation test area, construct a geomagnetic fingerprint database and fingerprint interface, and calculate step length measurement, heading angle measurement and geomagnetic measurement; Step 2: Construct a local attention factor graph model, fuse various measurements, and transform the navigation trajectory calculation problem into a constrained optimization problem. The specific steps are as follows: Step 2.1: Define the state transition model of the integrated navigation system: X i+1 =g(X i )+w i Where X i =[x i ,y i ] T It is the system state vector, x i and y i These represent two coordinate values ​​in a Cartesian coordinate system; g() is the state transition function, and w i It represents process noise, where i is the time series. Define the measurement model for the integrated navigation system: Z i =h(X i )+v i Z i It is a measurement vector, h() is a measurement function, and v i It is measurement noise; Step 2.2: Construct the factor node model: Where f is the factor node function, and L() is the error probability function. It is a prediction of the system state vector; Step 2.3: Introduce local attention to constrain the feasible region of factor nodes and reconstruct the factor graph model: Where X MAP It is the maximum a posteriori probability estimate of the state vector at all times, X = {X1, X2, ..., X...} i ...X n }, ||·|| denotes solving for the Mahalanobis distance, Σ is the residual covariance matrix, and δ i It is a constraint boundary; Step 2.4: Merge the measurement values: If we define a Gaussian probability distribution function to represent the error probability function L(), then the factor node model can be described as follows: Using a geomagnetic fingerprint database and a geomagnetic interface, geomagnetic estimates are obtained for any location within the region, and geomagnetic factors are constructed. Where f i mag It is the geomagnetic factor, mag i It is a geomagnetic measurement, h mag () is the geomagnetic interface function, Σ mag It is the geomagnetic error covariance matrix; The state transition model is rewritten as a measurement relation, and the heading angle measurement θ is used. i With step length measurement steplen i Described as: Where h step () is the step size measurement function. It is the step size measurement error; h angle () is the heading angle measurement function. This is the error in the heading angle measurement; further, the heading angles at adjacent times are differentially divided to obtain the differential heading angle Δθ. i Measurement model: Where h Dangle () is the differential heading angle measurement function. It is the error in the differential heading angle measurement; Assuming that the step size measurement errors at any two moments are independent and that the gyroscope offset errors within the same time interval follow the same probability distribution, then the step size factor f i steplen Sum of differential heading angle factor f i Dangle The description is as follows: Where Σ step It is the step size error covariance matrix, Σ Dangle It is the differential heading angle error covariance matrix; Step 2.5: Transform the navigation problem into a constrained optimization problem: Where |·| is the Euclidean distance function. It is the boundary of the maximum step size deviation. It is the boundary of the differential heading angle deviation; Since the magnetic field factor, the differential heading angle factor, and the step size factor are all in exponential form, the objective function can be equivalently represented in least squares form: Step 3: Use an improved genetic algorithm to solve the local attention factor graph model. Solve the optimal solution of the optimization problem through iterative methods of crossover, mutation, and selection, and decode to obtain the final navigation solution.

2. The geomagnetic / PDR indoor navigation method based on local attention factor maps as described in claim 1, characterized in that: Step 1 specifically involves: collecting magnetic field data in the navigation test area according to a grid and performing time averaging to construct a geomagnetic fingerprint database containing the magnitude of the magnetic field; then using the Bicubic interpolation method to interpolate and construct the geomagnetic fingerprint interface; next, continuously walking along the test trajectory in the navigation test area while simultaneously collecting magnetic field data and inertial device data; using a step length estimation algorithm to calculate the single-step length of the walk as the step length measurement value; and using a gait monitoring algorithm to locate the step timestamp and calculate the heading angle of each step in the walking trajectory as the heading angle measurement value. Calculate the magnitude of the geomagnetic field at the corresponding timestamp, and use it as the geomagnetic measurement value.

3. The geomagnetic / PDR indoor navigation method based on local attention factor maps as described in claim 1 or 2, characterized in that: The inertial device data includes triaxial magnetometer measurements, triaxial accelerometer measurements, and triaxial gyroscope measurements.

4. The geomagnetic / PDR indoor navigation method based on local attention factor maps as described in claim 1, characterized in that: The specific process of step 3 is as follows: Step 3.1, Chromosome Encoding and Decoding: Define the length of state X as n, and know the initial state X0 and the initial heading angle θ0; define the encoding method of a single chromosome as follows: C={c1,c2...c n ,c n+1 ...c 2n } Where C is the chromosome vector, consisting of 2n symbols, c k It is the allele at the k-th position; the first n alleles are step size alleles, and the (n+1)th to 2nth alleles are differential heading angle alleles; Define the following mapping relationship: Among them, c i c i+n Let represent the alleles at the i-th and (i+n)-th positions, respectively; It is an estimate of the step size. It is a step size estimate of the differential heading angle; allele c k The value range of is always limited to the interval (-1, 1). It is the deviation between the step size measurement and estimation. This is the deviation between the measurement and estimation of the differential heading angle; therefore, when the range of allele values ​​is always limited to the interval (-1, 1), The constraints in step 3.5 are always satisfied; Define the heading angle inverse model: in It is an estimate of the heading angle, where θ1 is the initial heading angle, j represents the time series and j < i; Define the state transition equation model: The above-mentioned mapping relationship, heading angle back-calculation model, and state transition equation model are collectively referred to as the chromosome decoding method. Through this decoding method, a single chromosome is uniquely decoded into an estimate of all states. Step 3.2, Define the fitness calculation method: The fitness of a single chromosome is calculated as follows: in and equivalence, and Equivalent, therefore the fitness function can be further simplified to: The method for calculating the fitness of multiple segments of a single chromosome is defined as follows: Among them, fitness′ d,e The fitness of segment de is where d and e are both positive integers and satisfy 1≤d≤e≤2n; the fitness of multiple segments is a standard for measuring the quality of chromosome segments. Step 3.3, Genetic Algorithm Initialization: 2n random real numbers are generated, ranging from -1 to 1. Each of these 2n random real numbers is represented as an allele, forming a chromosome. The maximum population size is set to `groupsize`. Each chromosome is considered an "individual". This process is repeated `groupsize` times to obtain `groupsize` initialized chromosomes, forming a population `group`. The p-th chromosome is described as C. p ; Step 3.4, Fitness Calculation: Different d and e values ​​are used to divide the chromosome into several segments: Where d w It is the index of the first allele in the w-th segment, e w It is the index of the last allele of the w-th segment, where w∈[1,mp]; calculate the fitness of each segment on each chromosome, and retain the optimal chromosome segment: Where D w It is the optimal chromosome segment among the w-th segments; by joining the ends of all the retained chromosome segments together, a new sequence B is formed: B={D1,D2...D mp-1 ,D mp } Since each segment in the sequence comes from a chromosome, the new sequence B is still a chromosome; clear the elite pool and put the new sequence B into the elite pool; Step 3.5, Chromosome Crossover: The fitness of each chromosome in the population group is calculated sequentially; the number of chromosomes to be retained in each iteration is set to parentnum, and selection is made based on the fitness value of individual chromosomes. The smaller the fitness value, the greater the probability of being selected. The chromosome with the lowest fitness is directly retained; the selected chromosomes are used as parent chromosomes, and the unselected chromosomes are directly eliminated and do not participate in subsequent iterations. Two different chromosomes C are randomly selected from the parent chromosomes. p1 C p2 Cross over: S k =p cross (C p1 ,C p2 ) in It is the l-th allele on the p-th chromosome; S k p indicates that crossover results in a new chromosome. cross () is the chromosome "crossover" function; g1 and g2 are randomly generated variables that satisfy the relation 1 ≤ g1 ≤ g2 ≤ 2n; S k All alleles in the chromosome are derived from the paternal chromosome C. p1 and C p2 Therefore, each allele is in the interval (-1,1); Repeat the crossover operation as groupsize-parentnum-1 times to generate groupsize-parentnum-1 new chromosomes. Define the newly generated chromosomes through the crossover operation as offspring chromosomes. Merge the parent and offspring chromosomes into a new population group. cross The number of individuals is groupsize-1; Step 3.6, Population Variation: Set the population group cross The mutation probability is Rate ∈ (0, 1); randomly generate a matrix with (groupsize - 1) rows and 2n columns, m row,col represents the element in the row row and column col of the matrix, and m row,col is a real number; for each m row,col , if m row,col < Rate, then let the col-th allele of the row-th chromosome in the population group cross where randgenerate() is a random number generation function that outputs a random real number with a value range of (-1, 1); traverse all m row,col cross , complete the mutation operation on the population groupAfter mutation, each allele in the population is in the interval (-1, 1); merge the population obtained after mutation with the sequence B in the elite pool to form a new population with groupsize individuals, and copy the values in the new population to group;​ Step 3.7, Iterative Solution Repeat steps 3.4 to 3.6, setting the maximum number of iterations to `generation`. Stop iterating when the number of iterations exceeds `generation`. Each iteration maintains a complete population. Select the chromosome with the lowest fitness in this population as the output of the genetic algorithm. Using the decoding method defined in step 4.1, decode the output of the genetic algorithm into a navigation solution: Where θ1 is the initial heading angle and X1 is the initial navigation state, both of which are known quantities; steplen i It is a step length measurement, △θ i It is a differential heading angle measurement; It is the final navigation solution after decoding.

Images