A method, system, electronic device and storage medium for orientation finding and positioning of digital training firearms.

Abstract

This invention discloses a digital training method, system, electronic device, and storage medium for direction finding and positioning of firearms, belonging to the field of simulation training. The method includes three steps: determining the firearm's operating environment and switching its operating state; processing and fusing sensor data; and outputting the firearm's heading and position. The sensor data processing and fusion process uses an error state Kalman filter algorithm to predict and correct the position and heading data. The system includes an IMU, GPS, and a computing unit. This invention can operate in three environments: low magnetic interference without GPS positioning, low magnetic interference with GPS positioning, and strong magnetic interference with GPS positioning, maximizing its adaptability to different environments. This invention fuses IMU sensor data and GPS sensor data, allowing the two sensors to complement each other's advantages. A state error Kalman filter is used to filter errors in the sensors, resulting in more accurate linearization of the fused data.

Description

Technical Field

[0001] This invention relates to the field of simulation training, and specifically to a method, system, electronic device, and storage medium for the direction finding and positioning of digital training firearms. Background Technology

[0002] With the transformation and upgrading of military training, the training methods of the armed forces have gradually shifted from live-fire exercises to simulated combat training. Currently, a common laser-based live-fire training model involves mounting laser transmitters on firearms and having soldiers wear specialized laser receivers to assess injury status based on laser strike behavior. However, this method's positioning and direction finding results are not very accurate and it cannot adapt to different environments. Summary of the Invention

[0003] To address the shortcomings of existing technologies, this invention provides a digital training firearms orientation and positioning method, system, electronic device, and storage medium. It utilizes an attitude sensor and a high-precision GPS sensor, along with accelerometer, gyroscope, geomagnetic, and latitude / longitude data, to ensure accurate recording of the aiming direction and position for each simulated shooting exercise.

[0004] To achieve the above objectives, the present invention adopts the following technical solution:

[0005] A method for direction finding and positioning of a digital training firearm, the method comprising the following steps:

[0006] Step 1: Determine whether the IMU is affected by magnetic field interference, determine whether the GPS positioning signal is affected by interference, and determine the working status of the training firearm; the working status includes: Status 1: low magnetic interference environment and no GPS positioning, Status 2: low magnetic interference environment and GPS positioning, Status 3: strong magnetic interference environment and GPS positioning, Status 4: strong magnetic interference environment and no GPS positioning.

[0007] Step 2: If the training firearm is in state one, calculate its heading and position by looking up the table based on the calibration stand and position coordinates. If the training firearm is in state two or three, use the error state Kalman filter algorithm to process the heading and position data. Specifically, use the position, velocity, and attitude information obtained from the IMU to predict data errors, including position error, velocity error, angle error, acceleration error, angular velocity error, and gravity error. Then, use the position and heading data obtained from GPS and the predicted data errors to correct and update the heading and position data. If the training firearm is in state four, the data is invalid.

[0008] Step 3: Output the heading and position of the training firearm.

[0009] To optimize the above technical solution, the specific measures also include:

[0010] Further, in step 1, determining whether the IMU is affected by magnetic field interference specifically involves: recording the magnetic induction data set of the current frame and the previous n frames at any time during system operation, counting the number of magnetic field strengths in the data set that are greater than a threshold, and considering the magnetic field interference as when the count is greater than 50% of the total number of data.

[0011] Furthermore, in step 1, determining whether the GPS positioning signal is interfered with specifically involves:

[0012] Calculate the latitude and longitude positioning data set of the current frame and the previous N frames during a short period of stillness. Calculate the average longitude value of the latitude and longitude positioning dataset. and average latitude value

[0013] Calculate the standard deviation δ of the longitude coordinates of all sample points in the latitude and longitude positioning dataset. xLon :

[0014]

[0015] In the formula, Represents the longitude coordinates, and N represents the number of data frames, which is also the number of samples in the latitude and longitude positioning dataset;

[0016] Calculate the standard deviation δ of the latitude coordinates of all sample points in the latitude and longitude positioning dataset. yLat :

[0017]

[0018] In the formula, Represents latitude coordinates;

[0019] With the average center of position coordinates Using the latitude and longitude positioning dataset as the center, calculate the standard deviation δ of the distances between all sample points in the dataset and the center. d :

[0020]

[0021] In the formula, d i This represents the distance between a sample point in the latitude and longitude positioning dataset and the center of the circle.

[0022] In the latitude and longitude positioning dataset, 95% of the sample points fall within a circle centered at the average center of the location coordinates, with a radius of R. 95 In the circle, calculate the radius R. 95 Value:

[0023] R 95 =1.227(δ) xLon +δ y Lat )

[0024] Determine the GPS positioning status; if the latitude and longitude coordinates fall within a radius of R 95 If the probability of the GPS signal falling within a circle is less than 95%, then the GPS is considered to have been interfered with during the process.

[0025] Furthermore, in step 2, the error in predicting the position, velocity, and attitude information obtained using the IMU specifically refers to:

[0026] Establish the state transition function f

[0027] f = F x (x est u m )δx+F i i

[0028] In the formula, δx represents the updated error, δx=[δp δv δθ δa b δω b δg] T δp is the position error, δv is the velocity error, δθ is the angle error, and δa is the position error. b For acceleration error, δω b δg is the angular velocity error, and δg is the gravity error;

[0029] F x Let x be the state transition matrix. est The six data points to be predicted are: position p, velocity v, angle θ, and acceleration a. b angular velocity ω b And gravity g, denoted as x est =[pv θ a] b ω b g] T ;u m The acceleration a measured by the IMU m With angular velocity ω m , represented as u m =[a m ω m ] T ;

[0030] F i Let i be the noise transition matrix, and i be the noise vector, i = [v i θ i a i ω i ] T v i For velocity noise, θ i For angular noise, a i For acceleration noise, ω i This is angular velocity noise;

[0031] State transition matrix F x The specific calculation formula is as follows:

[0032]

[0033] In the formula, Δt is the sampling interval, R is the observation noise, and I is the identity matrix;

[0034] Noise transfer matrix F i The specific calculation formula is as follows:

[0035]

[0036] In the formula, I is the identity matrix;

[0037] Update the covariance of the data error during the prediction phase:

[0038]

[0039] In the formula, P is the state covariance, and Q is... i The process noise matrix is ​​represented as follows:

[0040]

[0041] In the formula, V i It is the noise covariance of the velocity error δv, θ i A is the noise covariance of the pose error δθ. i It is the acceleration covariance, ω i It is the angular velocity covariance;

[0042]

[0043]

[0044] In the formula, Noise representing acceleration, This indicates angular velocity noise.

[0045] Further, in step 2, the step of correcting and updating the heading and position data using GPS-acquired position and heading data and predicted data errors specifically involves:

[0046] Calculate the gain K:

[0047] K = PH T (HPH T +R) -1

[0048] In the formula, R is the observation noise of GPS, and H is the measurement array, which is obtained through Jacobian relative to the error state;

[0049] Using the predicted data x est The corrected error is calculated using the observation data z from the GPS positioning and attitude sensors. The formula is as follows:

[0050]

[0051] Corrected error The updated error δx is continuously iterated in the input state transition function f;

[0052] Update the state covariance P:

[0053] P=(I-KH)P

[0054] In the formula, I is the identity matrix;

[0055] The state covariance P is input into the prediction stage and iterated continuously.

[0056] This invention also proposes a digital training firearm orientation and positioning system, which includes an IMU, a GPS, and a computing unit; the IMU measures position, velocity, and attitude information; the GPS measures position and heading data; the computing unit determines the operating state of the training firearm, processes the data measured by the IMU and GPS, and uses an error state Kalman filter algorithm to predict and correct the position and heading data.

[0057] The present invention also proposes an electronic device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, it implements the direction finding and positioning method for digital training firearms as described above.

[0058] The present invention also proposes a computer-readable storage medium, characterized in that it stores a computer program that enables a computer to execute the digital training firearm orientation and positioning method as described above.

[0059] The beneficial effects of this invention are:

[0060] This invention differs from traditional training methods that determine shot hits using laser transmitters and receivers. It solves the problem that the laser receiver fails to register a hit when the target is obscured by camouflage or trees. By using direction finding and position data obtained through digital methods, combined with map environment data, it determines whether the simulated shot hits, thus simulating combat situations more realistically.

[0061] The geomagnetic attitude sensor and GPS sensor used in this invention are respectively mounted on the training firearm, and their installation method is similar to that of the laser transmitter. Unlike the laser training mode, which requires the installation of laser receiving devices at various locations on the target, the digital mode directly obtains direction finding and position data and the calculation unit determines whether the simulated shooting hits.

[0062] This invention enables the acquisition of direction finding and location data in three different environments: low magnetic interference without GPS positioning, low magnetic interference with GPS positioning, and strong magnetic interference with GPS positioning, thereby maximizing the adaptability of this solution to different environments.

[0063] This invention fuses geomagnetic attitude sensor data with GPS sensor data, allowing the two sensors to complement each other's advantages. It employs a state error Kalman filter to filter the errors in the sensors. Because the errors are small, the linearization of the fused data will be more accurate. Attached Figure Description

[0064] Figure 1 A flowchart of the overall direction finding and positioning method for digital training firearms;

[0065] Figure 2 A flowchart for sensor data processing and fusion;

[0066] Figure 3 A flowchart for switching working states;

[0067] Figure 4 is a simulation diagram of the sensor installed on the training firearm. Figure 4a and Figure 4b This is a side view. Figure 4c This is the front view. Detailed Implementation

[0068] The invention will now be described in further detail with reference to the accompanying drawings.

[0069] In one embodiment, the present invention proposes a method for direction finding and positioning of digital training firearms, the overall flowchart of which is shown below. Figure 1 As shown, the specific steps include:

[0070] Step 1: Determine whether the IMU is affected by magnetic field interference, determine whether the GPS positioning signal is affected by interference, and determine the working status of the training firearm; the working status includes: Status 1: low magnetic interference environment and no GPS positioning, Status 2: low magnetic interference environment and GPS positioning, Status 3: strong magnetic interference environment and GPS positioning, Status 4: strong magnetic interference environment and no GPS positioning.

[0071] The process of determining whether the IMU is affected by magnetic field interference is as follows: at any time during system operation, record the magnetic induction data set of the current frame and the previous n frames, count the number of magnetic field strengths in the data set that are greater than the threshold, and when the count is greater than 50% of the total number of data, it is considered that the magnetic field is interfered with.

[0072] The process of determining whether a GPS positioning signal is being interfered with is as follows:

[0073] Calculate the latitude and longitude positioning data set of the current frame and the previous N frames during a short period of stillness. Multiple factors can cause the positioning point to deviate, resulting in deviations along the longitude X near the currently selected judgment point. lon Direction and latitude Y lat The distribution follows a normal pattern in both directions, and the scattering in these two directions can be considered the same and independent. Therefore, the average longitude value of the latitude and longitude positioning dataset is calculated. and average latitude value

[0074] Calculate the standard deviation δ of the longitude coordinates of all sample points in the latitude and longitude positioning dataset. xLon :

[0075]

[0076] In the formula, Represents the longitude coordinates, and N represents the number of data frames, which is also the number of samples in the latitude and longitude positioning dataset;

[0077] Calculate the standard deviation δ of the latitude coordinates of all sample points in the latitude and longitude positioning dataset. yLat :

[0078]

[0079] In the formula, Represents latitude coordinates;

[0080] With the average center of position coordinates Using the latitude and longitude positioning dataset as the center, calculate the standard deviation δ of the distances between all sample points in the dataset and the center. d :

[0081]

[0082] In the formula, d i This represents the distance between a sample point in the latitude and longitude positioning dataset and the center of the circle.

[0083] In the latitude and longitude positioning dataset, 95% of the sample points fall within a circle centered at the average center of the location coordinates, with a radius of R. 95 In the circle, calculate the radius R. 95 Value:

[0084] R95 =1.227(δ) xLon +δ yLat )

[0085] Determine the GPS positioning status; if the latitude and longitude coordinates fall within a radius of R 95 If the probability of the GPS signal falling within a circle is less than 95%, then the GPS is considered to have been interfered with during the process.

[0086] Digital training firearms adaptively switch their operating states according to their working environment. The state switching process is as follows: Figure 3 As shown.

[0087] Step 2: If the training firearm is in state one, then calculate the heading and position of the training firearm by referring to the table based on the calibration stand and position coordinates.

[0088] If the training firearm is in state two or state three, then the error state Kalman filter algorithm is used to process the heading and position data. This process is as follows: Figure 2 As shown, specifically, the position, velocity, and attitude information obtained using IM(j) is used to predict data errors. These data errors include position error, velocity error, angle error, acceleration error, angular velocity error, and gravity error. The specific process is as follows:

[0089] Establish the state transition function f

[0090] f = F x (x est u m )δx+F i i

[0091] In the formula, δx represents the updated error, δx=[δp δv δθ δa b δω b δg] T δp is the position error, δv is the velocity error, δθ is the angle error, and δa is the position error. b For acceleration error, δω b δg is the angular velocity error, and δg is the gravity error;

[0092] F x Let x be the state transition matrix. est The six data points to be predicted are: position p, velocity v, angle θ, and acceleration a. b angular velocity ω b And gravity g, denoted as x est =[pv θ a] b ω b g] T ;u m The acceleration a measured by the IMU m With angular velocity ωm , represented as u m =[a m ω m ] T ;

[0093] F i Let i be the noise transition matrix, and i be the noise vector, i = [v i θ i a i ω i ] T v i For velocity noise, θ i For angular noise, a i For acceleration noise, ω f This is angular velocity noise;

[0094] State transition matrix F x The specific calculation formula is as follows:

[0095]

[0096] In the formula, Δt is the sampling interval, R is the observation noise, and I is the identity matrix;

[0097] Noise transfer matrix F i The specific calculation formula is as follows:

[0098]

[0099] In the formula, I is the identity matrix;

[0100] Update the covariance of the data error during the prediction phase:

[0101]

[0102] In the formula, P is the state covariance, and Q is... i The process noise matrix is ​​represented as follows:

[0103]

[0104] In the formula, V i It is the noise covariance of the velocity error δv, θ i A is the noise covariance of the pose error δθ. i It is the acceleration covariance, ω i It is the angular velocity covariance;

[0105]

[0106]

[0107] In the formula, Noise representing acceleration, This indicates angular velocity noise.

[0108] The GPS-acquired position and heading data, along with the predicted data, are then used to correct for errors and update the heading and position data; the specific process is as follows:

[0109] Calculate the gain K:

[0110] K = PH T (HPH T +R) -1

[0111] In the formula, R is the observation noise of GPS, and H is the measurement array, which is obtained through Jacobian relative to the error state;

[0112] Using the predicted data x est The corrected error is calculated using the observation data z from the GPS positioning and attitude sensors. The formula is as follows:

[0113]

[0114] Corrected error The updated error δx is continuously iterated in the input state transition function f;

[0115] Update the state covariance P:

[0116] P=(I-KH)P

[0117] In the formula, I is the identity matrix;

[0118] The state covariance P is input into the prediction stage and iterated continuously. By alternating between prediction and measurement updates, a closed loop for data state estimation is achieved.

[0119] If the training firearm is in state four, the data is invalid;

[0120] Step 3: Output the heading and position of the training firearm.

[0121] In another embodiment, the present invention proposes a direction finding and positioning system for a digital training firearm. The system includes an IMU, a GPS, and a computing unit. The IMU measures position, velocity, and attitude information; the GPS measures position and heading data; the computing unit determines the operating state of the training firearm, processes the data measured by the IMU and GPS, and uses an error-state Kalman filter algorithm to predict and correct the position and heading data. The sensor installation and calibration process is as follows:

[0122] The sensor and processing unit are built into the green L-shaped equipment compartment shown in the figure and are fixed to the gun by clamps. See Figure 4 for an installation diagram.

[0123] When calibrating the sensor, it is necessary to establish a right-handed spatial coordinate system O1 with the midpoint of the firearm as the origin and the forward direction as the X-axis, with the X-axis as the positive direction. The GPS sensor coordinates recorded in the O1 coordinate system are O... 1gps (x, y, z) and its coordinate system is denoted as O. gps The IMU sensor coordinates recorded in the O1 coordinate system are O 1imu (x, y, z) and its coordinate system is denoted as O. imu Used for conversion between different coordinate systems of GPS sensors and IMU sensors.

[0124] In another embodiment, the present invention provides an electronic device comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, it implements the direction finding and positioning method for digital training firearms as described above.

[0125] In another embodiment, the present invention also provides a computer-readable storage medium, characterized in that it stores a computer program that causes a computer to execute the direction finding and positioning method for digital training firearms as described above.

[0126] It should be noted that the terms such as "upper", "lower", "left", "right", "front", and "back" used in the invention are only for clarity of description and are not intended to limit the scope of the invention. Changes or adjustments to their relative relationships, without substantially altering the technical content, should also be considered within the scope of the invention.

[0127] In the embodiments disclosed in this application, a computer storage medium may be a tangible medium that may contain or store programs for use by or in conjunction with an instruction execution system, apparatus, or device. The computer storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, apparatus, or devices, or any suitable combination of the foregoing. More specific examples of computer storage media include electrical connections based on one or more wires, portable computer disks, hard disks, random access memory (RAM), read-only memory (ROM), erasable programmable read-only memory (EPROM or flash memory), optical fibers, portable compact disk read-only memory (CD-ROM), optical storage devices, magnetic storage devices, or any suitable combination of the foregoing.

[0128] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this application can be implemented in electronic hardware or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0129] The above are merely preferred embodiments of the present invention. The scope of protection of the present invention is not limited to the above embodiments. All technical solutions falling within the scope of the present invention's concept are within the scope of protection of the present invention. It should be noted that for those skilled in the art, any improvements and modifications made without departing from the principles of the present invention should be considered within the scope of protection of the present invention.

Claims

1. A method for direction finding and positioning of a digital training firearm, characterized in that, The method includes the following steps: Step 1: Determine if the IMU is affected by magnetic field interference, determine if the GPS positioning signal is affected by interference, and determine the working state of the training firearm; the working states include: State 1: low magnetic interference environment and no GPS positioning; State 2: low magnetic interference environment and GPS positioning; State 3: strong magnetic interference environment and GPS positioning; State 4: strong magnetic interference environment and no GPS positioning; In Step 1, determining if the GPS positioning signal is affected by interference specifically involves: Calculate the latitude and longitude positioning data set of the current frame and the previous N frames during a short period of stillness. Calculate the average longitude value of the latitude and longitude positioning dataset. and average latitude value Calculate the standard deviation δ of the longitude coordinates of all sample points in the latitude and longitude positioning dataset. xLon : In the formula, Represents the longitude coordinates, and N represents the number of data frames, which is also the number of samples in the latitude and longitude positioning dataset; Calculate the standard deviation δ of the latitude coordinates of all sample points in the latitude and longitude positioning dataset. yLat : In the formula, Represents latitude coordinates; With the average center of position coordinates Using the latitude and longitude positioning dataset as the center, calculate the standard deviation δ of the distances between all sample points in the dataset and the center. d : In the formula, d i This represents the distance between a sample point in the latitude and longitude positioning dataset and the center of the circle. In the latitude and longitude positioning dataset, 95% of the sample points fall within a circle centered at the average center of the location coordinates, with a radius of R. 95 In the circle, calculate the radius R. 95 Value: R 95 =1.227(d xLon +d yLat ) Determine the GPS positioning status; if the latitude and longitude coordinates fall within a radius of R 95 If the probability of the GPS signal falling within a circle is less than 95%, then the GPS is considered to have been interfered with during the process. Step 2: If the training firearm is in state one, calculate its heading and position by looking up the table based on the calibration stand and position coordinates. If the training firearm is in state two or three, use the error state Kalman filter algorithm to process the heading and position data. Specifically, use the position, velocity, and attitude information obtained from the IMU to predict data errors, including position error, velocity error, angle error, acceleration error, angular velocity error, and gravity error. Then, use the position and heading data obtained from GPS and the predicted data errors to correct and update the heading and position data. If the training firearm is in state four, the data is invalid. In step 2, the error in predicting the position, velocity, and attitude information obtained using the IMU specifically refers to: Establish the state transition function f f=F x (x est ,u m )δx+F i i In the formula, δx represents the updated error, δx=[δp δv δθ δa b δω b δg] T δp is the position error, δv is the velocity error, δθ is the angle error, and δa is the position error. b For acceleration error, δω b δg is the angular velocity error, and δg is the gravity error; F x Let x be the state transition matrix. est The six data points to be predicted are: position p, velocity v, angle θ, and acceleration a. b angular velocity ω b And gravity g, denoted as x est =[pv θ a] b ω b g] T ;u m The acceleration a measured by the IMU m With angular velocity ω m , represented as u m =[a m ω m ] T ; F i Let i be the noise transition matrix, and i be the noise vector, i = [v i θ i a i ω i ] T v i For velocity noise, θ i For angular noise, a i For acceleration noise, ω i This is angular velocity noise; State transition matrix F x The specific calculation formula is as follows: In the formula, Δt is the sampling interval, R is the observation noise, and I is the identity matrix; Noise transfer matrix F i The specific calculation formula is as follows: In the formula, I is the identity matrix; Update the covariance of the data error during the prediction phase: In the formula, P is the state covariance, and Q is... i The process noise matrix is ​​represented as follows: In the formula, V i It is the noise covariance of the velocity error δv, θ i A is the noise covariance of the pose error δθ. i It is the acceleration covariance, ω i It is the angular velocity covariance; In the formula, Noise representing acceleration, Indicates angular velocity noise; In step 2, the step of correcting and updating the heading and position data using GPS-acquired position and heading data and predicted data errors specifically involves: Calculate the gain K: K=PH T (HPH T +R) -1 In the formula, R is the observation noise of GPS, and H is the measurement array, which is obtained through Jacobian relative to the error state; Using the predicted data x est The corrected error is calculated using the observation data z from the GPS positioning and attitude sensors. The formula is as follows: Corrected error The updated error δx is continuously iterated in the input state transition function f; Update the state covariance P: P=(I-KH)P In the formula, I is the identity matrix; Input the state covariance P into the prediction stage and iterate continuously; Step 3: Output the heading and position of the training firearm.

2. The direction finding and positioning method for digital training firearms according to claim 1, characterized in that, In step 1, determining whether the IMU is affected by magnetic field interference specifically involves: recording the magnetic induction data set of the current frame and the previous n frames at any time during system operation, counting the number of data sets with magnetic field strength greater than a threshold, and considering the magnetic field interference as when the count is greater than 50% of the total number of data sets.

3. A digital training firearms orientation and positioning system, characterized in that, The system includes an IMU, a GPS, and a computing unit; the IMU measures position, velocity, and attitude information; the GPS measures position and heading data; the computing unit determines the working status of the training firearm, processes the data measured by the IMU and GPS, and uses an error state Kalman filter algorithm to predict and correct the position and heading data; The calculation unit determines the working state of the training firearm by: determining whether the IMU is affected by magnetic field interference, determining whether the GPS positioning signal is affected by interference, and determining the working state of the training firearm; the working state includes state one: low magnetic interference environment and no GPS positioning, state two: low magnetic interference environment and GPS positioning, state three: strong magnetic interference environment and GPS positioning, and state four: strong magnetic interference environment and no GPS positioning. The specific steps for determining whether the GPS positioning signal is interfered with are as follows: Calculate the latitude and longitude positioning data set of the current frame and the previous N frames during a short period of stillness. Calculate the average longitude value of the latitude and longitude positioning dataset. and average latitude value Calculate the standard deviation δ of the longitude coordinates of all sample points in the latitude and longitude positioning dataset. xLon : In the formula, Represents the longitude coordinates, and N represents the number of data frames, which is also the number of samples in the latitude and longitude positioning dataset; Calculate the standard deviation δ of the latitude coordinates of all sample points in the latitude and longitude positioning dataset. yLat : In the formula, Represents latitude coordinates; With the average center of position coordinates Using the latitude and longitude positioning dataset as the center, calculate the standard deviation δ of the distances between all sample points in the dataset and the center. d : In the formula, d i This represents the distance between a sample point in the latitude and longitude positioning dataset and the center of the circle. In the latitude and longitude positioning dataset, 95% of the sample points fall within a circle centered at the average center of the location coordinates, with a radius of R. 95 In the circle, calculate the radius R. 95 Value: R 95 =1.227(d xLon +d yLat ) Determine the GPS positioning status; if the latitude and longitude coordinates fall within a radius of R 95 If the probability of the GPS signal falling within a circle is less than 95%, then the GPS is considered to have been interfered with during the process. The use of the error state Kalman filter algorithm to predict and correct position and heading data specifically involves using position, velocity, and attitude information obtained from the IMU to predict data errors, including position error, velocity error, angle error, acceleration error, angular velocity error, and gravity error; then using position and heading data obtained from GPS and the predicted data errors to correct and update the heading and position data. The specific errors in the prediction data using the position, velocity, and attitude information obtained from the IMU are as follows: Establish the state transition function f f=F x (x est ,u m )δx+F i i In the formula, δx represents the updated error, δx=[δp δv δθ δa b δω b δg] T δp is the position error, δv is the velocity error, δθ is the angle error, and δa is the position error. b For acceleration error, δω b δg is the angular velocity error, and δg is the gravity error; F x Let x be the state transition matrix. est The six data points to be predicted are: position p, velocity v, angle θ, and acceleration a. b angular velocity ω b And gravity g, denoted as x est =[pv θ a] b ω b g] T ;u m The acceleration a measured by the IMU m With angular velocity ω m , represented as u m =[a m ω m ] T ; F i Let i be the noise transition matrix, and i be the noise vector, i = [v i θ i a i ω i ] T v i For velocity noise, θ i For angular noise, a i For acceleration noise, ω i This is angular velocity noise; State transition matrix F x The specific calculation formula is as follows: In the formula, Δt is the sampling interval, R is the observation noise, and I is the identity matrix; Noise transfer matrix F i The specific calculation formula is as follows: In the formula, I is the identity matrix; Update the covariance of the data error during the prediction phase: In the formula, P is the state covariance, and Q is... i The process noise matrix is ​​represented as follows: In the formula, V i It is the noise covariance of the velocity error δv, θ i A is the noise covariance of the pose error δθ. i It is the acceleration covariance, ω i It is the angular velocity covariance; In the formula, Noise representing acceleration, Indicates angular velocity noise; The specific steps for correcting and updating the heading and position data using GPS-acquired position and heading data and predicted data errors are as follows: Calculate the gain K: K=PH T (HPH T +R) -1 In the formula, R is the observation noise of GPS, and H is the measurement array, which is obtained through Jacobian relative to the error state; Using the predicted data x est The corrected error is calculated using the observation data z from the GPS positioning and attitude sensors. The formula is as follows: Corrected error The updated error δx is continuously iterated in the input state transition function f; Update the state covariance P: P=(I-KH)P In the formula, I is the identity matrix; The state covariance P is input into the prediction stage and iterated continuously.

4. An electronic device, characterized in that, include: The device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor, when executing the computer program, implements the orientation and positioning method for a digital training firearm as described in any one of claims 1-2.

5. A computer-readable storage medium, characterized in that: The computer program stores a computer program that causes the computer to perform the direction finding and positioning method for the digital training firearm as described in any one of claims 1-2.

Images