Digital Kalman gain rapid calculation method for time-sharing uniformly-added accelerated radar target

Abstract

The invention discloses a digital Kalman gain rapid calculation method for a time-sharing uniformly-accelerated radar target, which comprises the following steps of: further coupling a Kalman gain calculation formula at the kth moment and a state one-step prediction covariance equation at the (k-1) th moment by extracting the characteristics of a prediction equation and an update equation in Kalman filtering, and calculating a matrix HPk in advance to obtain the Kalman gain of the kth moment. And the k-1HT + Rk achieves an optimization effect. According to the method, the distribution rule of inverse matrix elements of a matrix HPk, k-1HT + Rk is deeply mined by utilizing the mathematical characteristics of the matrix HPk, k-1HT + Rk, so that the rapid calculation of the inverse matrix is realized. The improvement greatly improves the calculation speed of the Kalman gain. Compared with a traditional Kalman filtering algorithm, the method shows greater potential in the aspect of parallel calculation, the calculation complexity is effectively reduced, and the execution speed of the Kalman filtering process is remarkably increased. According to the invention, a more efficient solution is provided for real-time tracking and prediction of the space target, and the method has significant advantages especially in a scene where computing resources are limited or high-frequency updating is needed.

Description

Technical Field

[0001] The invention belongs to the field of radar technology, and in particular relates to a method for quickly calculating a digital Kalman gain of a time-sharing uniformly accelerated radar target. Background Art

[0002] Kalman filtering is a recursive algorithm used to estimate the state of a dynamic system, especially in systems with noise or uncertainty. This algorithm is very suitable for target tracking and trajectory prediction because it can optimize the estimate of the target's state based on current observations and estimates of past states, thereby reducing the impact of noise on the system.

[0003] One of the challenges in target tracking is that sensor data contains noise, which leads to inaccurate measurement results. Kalman filtering can make a more accurate estimate of the target's true state by modeling and recursively estimating noise. In the trajectory prediction problem, the Kalman filter can predict the target's trajectory in the future based on the past motion state and current observations. It recursively updates the target's state (such as position and velocity) and predicts the state at the next moment or longer based on the motion model.

[0004] The Kalman filter needs to update the Kalman gain matrix in each cycle, which is a key factor in the state estimation process. But there is a common challenge - the Kalman gain matrix is ​​computationally intensive, especially in high-dimensional scenarios. This is because the update steps of the covariance matrix and Kalman gain in the Kalman filter involve matrix operations (including matrix inversion), which may lead to inefficient calculations when the dimensions are large. The calculation of the Kalman gain involves matrix inversion operations, which usually has a computational complexity of O(n 3 )(where n is the dimension of the state variable), especially in high-dimensional space applications, such as multi-target tracking or trajectory prediction of complex systems, the calculation time will increase dramatically, resulting in low efficiency for systems with high real-time requirements.

[0005] Since the Kalman gain calculation requires the inversion of the matrix HP k,k-1 H T +R kIt is a symmetric positive definite matrix, so it can be solved by matrix decomposition. In Kalman filtering, the symmetric positive definite matrix can be solved by Cholesky decomposition, an efficient decomposition method for symmetric positive definite matrices. This method has high numerical stability and is faster than direct inversion, but it may still not meet the real-time requirements in large-scale high-dimensional systems, and the decomposition cannot be completed when the numerical value is unstable. Extended Kalman filtering is a nonlinear extension of Kalman filtering, which is used to deal with nonlinear systems. Although it is not specifically designed to solve the problem of Kalman gain matrix calculation, in many practical applications, linearization reduces the computational complexity of the gain matrix. Extended Kalman filtering requires linearization in each iteration, which may affect the accuracy of the calculation, especially for high-dimensional systems or highly nonlinear systems, the amount of matrix calculation is still large. In some complex high-dimensional systems, the state transfer matrix or observation matrix may be a sparse matrix. The use of sparse matrix technology can significantly reduce the amount of calculation, because in a sparse matrix, only a few elements are non-zero, and the complexity of matrix operations will be significantly reduced. This method is only applicable to situations where the dynamic characteristics of the system change relatively locally. Summary of the invention

[0006] In order to overcome the shortcomings of the prior art, the present invention provides a method for quickly calculating the digital Kalman gain of a time-sharing uniform acceleration radar target. By extracting the characteristics of the prediction equation and the update equation in the Kalman filter, the Kalman gain calculation formula at the kth moment is further coupled with the state one-step prediction covariance equation at the k-1th moment. By calculating the matrix HP in advance, k,k-1 H T +R k To achieve the optimization effect. This method uses the matrix HP k,k-1 H T +R k The mathematical characteristics of the inverse matrix are deeply explored to explore the distribution law of its inverse matrix elements, thereby realizing the rapid calculation of the inverse matrix. This improvement greatly improves the calculation speed of the Kalman gain. Compared with the traditional Kalman filter algorithm, the present invention shows greater potential in parallel computing, effectively reduces the computational complexity, and significantly speeds up the execution speed of the Kalman filter process. The present invention provides a more efficient solution for real-time tracking and prediction of space targets, especially in scenarios where computing resources are limited or high-frequency updates are required.

[0007] The technical solution adopted by the present invention to solve the technical problem is as follows:

[0008] Step 1: When tracking a target based on radar and using Kalman filtering to predict the trajectory of a target that is moving in time-sharing uniform acceleration in space, first determine the system state, establish the motion state extrapolation equation, state transfer matrix, and measurement matrix on the three degrees of freedom of the target space, and determine the measurement noise matrix and process noise matrix;

[0009] For three degrees of freedom in space, the system state vector is determined as follows:

[0010]

[0011] in, represents the system state estimation vector at the kth moment, and a rectangular coordinate system is established with the center of the earth as the origin, where (x, y, z) represents the position of the observed target in three degrees of freedom, (v x ,v y ,v z ) represents the velocity of the observed target in three degrees of freedom, (a x ,a y ,a z ) represents the acceleration of the three degrees of freedom of the observed target, (j x ,j y ,j z ) represents the acceleration of the three degrees of freedom of the observed target;

[0012] The extrapolation equation of the target motion state with uniform acceleration is shown in equation (2). The system state vector is converted from the estimated vector at the current time k to Extrapolation The prediction vector for the future time k+1 Where Δt represents the time step, They represent the estimated values ​​of the position (x, y, z) at the kth moment, respectively. represent the position at the kth moment (v x ,v y ,v z ), represent the position at the kth moment (a x ,a y ,a z ), represent the position at the kth moment (j x ,j y ,j z ), They represent (x, y, z), (v x ,v y ,v z )、(a x ,a y,a z )、(j x ,j y ,j z )’s predicted value:

[0013]

[0014] The state transfer matrix is, where Δt represents the time step:

[0015]

[0016] Assuming that the observable state has position and velocity in three degrees of freedom, the measurement matrix is ​​as follows:

[0017]

[0018] In Kalman filtering, assuming that the process noise covariance matrix Q and the measurement noise covariance matrix R are constants, the noise in the Kalman filtering process is zero-mean Gaussian white noise, and the noises on the three degrees of freedom are independent of each other, then the measurement noise covariance matrix is ​​as follows, where r ab represents the measurement noise covariance of a and b, such as is the position x and velocity v in the x degree of freedom x Covariance of measurement noise:

[0019]

[0020] The process noise covariance matrix is ​​as follows, where σ ab represents the process noise covariance of a and b, such as is the position x and velocity v in the x degree of freedom x Covariance of process noise:

[0021]

[0022] Step 2: The measurement prediction mean square error matrix HP that needs to be inverted k,k-1 H T The form of +R is as follows:

[0023]

[0024] Where a, b, c, d, e, f, x, y, z are in the following form, and the value of each can be calculated from the elements in the state estimation mean square error matrix Θ, the process noise covariance matrix Q and the measurement covariance matrix at the k-1th moment;

[0025] The form of Θ is as follows, where p ab Represents the state estimation covariance of a and b, such as p ab is the position x and velocity v in the x degree of freedomx The covariance of the state estimate, P k-1,k-1 is the state estimation covariance matrix of the k-1th step:

[0026]

[0027] The expressions of a,b,x,d,e,f,x,y,z are as follows, where Θ (x,y) represents the element at position (x,y) of the matrix Θ, and Δt represents the time step:

[0028] ●a:

[0029]

[0030]

[0031] b:

[0032]

[0033] c:

[0034]

[0035] ·d:

[0036]

[0037] e:

[0038]

[0039] f:

[0040]

[0041] ●x

[0042]

[0043] y:

[0044]

[0045] z:

[0046]

[0047] Step 3: Calculate the measurement prediction mean square error matrix H k P k,k-1 H T +R k The inverse matrix of

[0048] The inverse matrix obtained by Gaussian elimination method is as follows:

[0049]

[0050] Step 4: P k,k-1 H T After derivation, the form is as follows:

[0051]

[0052] where Φ x,y It is a 3*3 diagonal matrix;

[0053] Step 5: Substitute the matrix (HP k,k-1 H T +R) -1 And the matrix P obtained in step 4 k,k-1 H T Perform matrix multiplication to obtain the Kalman gain K k ;

[0054] Then use the obtained Kalman gain to update the state equation Estimate current status Using uncertainty to update the equation Update the current estimated uncertainty P k,k .

[0055] Step 6: Repeat Step 2-Step 5 to continuously estimate the state of the system.

[0056] A computer program enables a computer to execute the above-mentioned digital Kalman gain fast calculation method.

[0057] An electronic device comprises: a processor and a memory; the memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the electronic device executes the above-mentioned digital Kalman gain fast calculation method.

[0058] A computer-readable storage medium stores a computer program, which implements the above-mentioned digital Kalman gain fast calculation method when executed by a processor.

[0059] A chip includes: a processor for calling and running a computer program from a memory, so that a device equipped with the chip executes the above-mentioned digital Kalman gain fast calculation method.

[0060] A computer program product, the computer program product comprising a computer storage medium, the computer storage medium storing a computer program, the computer program comprising instructions executable by at least one processor, and the above-mentioned digital Kalman gain fast calculation method is implemented when the instructions are executed by the at least one processor.

[0061] The beneficial effects of the present invention are as follows:

[0062] The present invention provides a digital Kalman fast calculation method. On the basis of mathematical derivation, the method realizes a calculation channel from a covariance state estimation mean square matrix at the k-1th moment directly to a Kalman gain matrix by simplifying the calculation, omitting the storage and calculation of intermediate results, reducing the calculation complexity, and speeding up the Kalman filtering process, so that the method is more suitable for systems with high real-time requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0063] Figure 1 It is a flowchart of the method of the present invention. DETAILED DESCRIPTION

[0064] The present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0065] In radar target tracking systems, Kalman filtering is often used for data fusion and trajectory prediction. When performing trajectory prediction, the tracking target is regarded as performing uniform acceleration motion, which can make the prediction result more accurate, but it will also increase the data dimension and computational complexity, especially the calculation of Kalman gain. In order to reduce the computational complexity of Kalman gain and improve the concurrency of Kalman gain calculation, the present invention uses advance calculation combined with the distribution characteristics of matrix elements to propose a Kalman gain fast calculation method, which further couples the covariance state estimation mean square matrix at the k-1th moment and the Kalman gain calculation equation, and after mathematical derivation and proof, a simplified calculation is performed to quickly obtain the inverse matrix of the measurement prediction mean square error matrix, thereby reducing the amount of calculation and speeding up the calculation process of Kalman gain.

[0066] In the field of radar target tracking, accurate estimation of target position, velocity, acceleration, and jerk is crucial to the stability and safety of air defense warning, navigation and guidance systems. However, radar signals are often interfered by noise, and the uncertainty of observation data is high, which makes it more difficult to estimate the target state. In order to achieve high-precision target tracking, the system needs a technology that can extract reliable state information from the noise.

[0067] Kalman filtering has become the core algorithm in radar target tracking due to its excellent recursive estimation and noise suppression capabilities. Kalman filtering uses prior information of the target state and current observation data to continuously correct the estimated value of the target through a cycle of prediction and update. It can optimize the real-time estimation of the target state in a high-noise environment, making tracking more accurate and stable. In engineering, the application of Kalman filtering has greatly improved the performance of the radar system, enabling the system to accurately track fast-moving targets in a dynamic environment.

[0068] When tracking targets based on radar and using Kalman filtering to predict the trajectory of targets that are performing time-sharing uniformly accelerated motion in space, it is necessary to first determine the system state, establish the motion state extrapolation equations in the three degrees of freedom of the target space, the state transfer matrix, the measurement matrix, and determine the measurement noise matrix and the process noise matrix.

[0069] When using Kalman filtering to track and predict the trajectory of a time-sharing uniformly accelerated target in space, it is necessary to first determine the system state, establish the motion state extrapolation equation in the three degrees of freedom of the target space, the state transfer matrix, the measurement matrix, and determine the measurement noise matrix and the process noise matrix.

[0070] Taking target tracking for drones as an example, for the three degrees of freedom x, y, z in space, the system state vector is expressed as follows:

[0071] For three degrees of freedom in space, the system state vector is determined as follows:

[0072]

[0073] represents the system state estimation vector at the kth moment, and a rectangular coordinate system is established with the center of the earth as the origin, where (x, y, z) represents the position of the observed target in three degrees of freedom, (v x ,v y ,v z ) represents the velocity of the observed target in three degrees of freedom, (a x ,a y ,a z ) represents the acceleration of the three degrees of freedom of the observed target, (j x ,j y ,j z ) represents the acceleration of the three degrees of freedom of the observed target;

[0074] The extrapolation equation of the target motion state with uniform acceleration is shown in equation (2). The system state vector is converted from the estimated vector at the current time k to Extrapolation The prediction vector for the future time k+1 Where Δt represents the time step, They represent the estimated values ​​of the position (x, y, z) at the kth moment, respectively. represent the position at the kth moment (v x ,v y ,v z ), represent the position at the kth moment (a x ,a y ,a z ), represent the position at the kth moment (j x ,j y ,j z ), They represent (x, y, z), (v x ,v y ,v z )、(a x ,a y ,a z )、(j x ,j y ,j z )’s predicted value:

[0075]

[0076] The system state at time k is transferred to time k+1(X k+1,k =FX k,k ) is as follows, where Δt represents the time step:

[0077]

[0078] The observable state of the drone has position and velocity in three degrees of freedom, so the measurement matrix is:

[0079]

[0080] The noise of Kalman filtering is zero-mean Gaussian white noise, and the noises on the three degrees of freedom are independent of each other, and the process noise and measurement noise are uncorrelated, satisfying:

[0081] The mean of the noise is zero, that is, E[n(t)] = 0

[0082] The distribution of noise conforms to the Gaussian (normal) distribution, usually expressed by the standard deviation σ: n(t)~

[0083] N(0,σ 2 ).

[0084] Among them, the variance σ 2Describes the intensity of the noise.

[0085] Noise is independent in time, and there is no correlation between the noise values ​​at any two different time points. The autocorrelation function is:

[0086] R(n(t1),n(t2))=σ 2 δ(t1-t2)

[0087] Wherein, δ is the Dirac function, which means it is non-zero when t1=t2 and zero at other times.

[0088] Assume that σ is the process noise and r is the measurement noise, and both satisfy:

[0089] Cov(σ,r)=E[(σ-E[σ])(rE[r])]=0

[0090] In Kalman filtering, designers usually assume that the process noise covariance matrix Q and the measurement noise covariance matrix R are constants because the time-varying noise covariance matrix is ​​difficult to determine and introduces additional computational burden.

[0091] When the three-degree-of-freedom noises are independent of each other, the covariance of the measurement noise on any two degrees of freedom is 0, that is, r xy =0, So the measurement noise covariance matrix can be simplified as follows, where r ab represents the measurement noise covariance of a and b, such as is the position x and velocity v in the x degree of freedom x Covariance of measurement noise:

[0092]

[0093] When the three-degree-of-freedom noises are independent of each other, the covariance of the process noise on any two degrees of freedom is 0, that is, σ xy =0,σ xz =0, So the process noise covariance matrix can be simplified as follows, where σ ab represents the process noise covariance of a and b, such as is the position x and velocity v in the x degree of freedom x Covariance of process noise:

[0094]

[0095] In practical applications, a certain filtering process will only be a sample of the overall random process, and the true value of the initial value of the filtering state is often unknown, so the initial value of the filtering state is generally set to a value near the true value, and sometimes even directly set to the zero vector. Therefore, in practice, the estimation result of the Kalman filter is always biased, but as long as the filtering system is asymptotically stable, the influence of the initial value will gradually disappear as the number of filtering steps increases.

[0096] The initial drone state vector is:

[0097]

[0098] The initial covariance matrix is:

[0099]

[0100] The following content is extrapolated from the k-1th moment to the kth moment, setting the state estimation mean square error matrix P at the k-1th moment k-1,k-1 For, where p ab represents the state covariance of a and b, such as is the position x and velocity v in the x degree of freedom x Covariance of state estimates:

[0101]

[0102] Because the noises on any two degrees of freedom are independent of each other, the covariance of the noises on any two degrees of freedom is zero.

[0103] As the four steps described in the technical solution, step 1 is implemented as follows:

[0104] Using the above determined conditions, after mathematical derivation, it can be concluded that the measurement prediction mean square error matrix HP that needs to be inverted is k,k-1 H T The form of +R is as follows:

[0105]

[0106] The form of a, b, c, d, e, f, x, y, z is as follows, and the value of each can be calculated from the elements in the state estimation mean square error matrix Θ, the process noise covariance matrix Q, and the measurement noise covariance matrix at the kth moment; the form of Θ is as follows:

[0107] Θ=P k-1,k-1

[0108]

[0109] The form and calculation method of a, b, c, d, e, f, x, y, z in the measurement prediction mean square error matrix are as follows, where Θ (x,y) represents the element at position (x,y) of the matrix Θ, and Δt represents the time step:

[0110] ·a

[0111] ●b

[0112]

[0113] ●c

[0114]

[0115] a is calculated as follows:

[0116] variable:

[0117] Θ (1,1) ,Θ (4,1) ,Θ (7,1) ,Θ (4,4) ,Θ (10,1) ,Θ (7,4) ,Θ (10,4) ,Θ (7,7) ,Θ (10,7) ,Θ (10,10) ,Q (1,1) ,R (1,1) Fixed parameters:

[0118]

[0119] addition:

[0120]

[0121] Constant multiplication:

[0122]

[0123] addition:

[0124]

[0125] multiplication:

[0126]

[0127] addition:

[0128]

[0129] addition:

[0130]

[0131] addition:

[0132]

[0133] Replace the input with:

[0134] Θ (2,2) ,Θ (5,2) ,Θ (8,2) ,Θ (5,5) ,Θ (11,2) ,Θ (8,5) ,Θ (11,5) ,Θ (8,8) ,Θ (11,8) ,Θ (11,11) ,Q (2,2) ,R (2,2) Similarly, we can get b.

[0135] Replace the input with:

[0136] Θ (3,3) ,Θ (6,3) ,Θ (9,3) ,Θ (6,6) ,Θ (12,3) ,Θ (9,6) ,Θ (12,6) ,Θ (9,9) ,Θ (12,9) ,Θ (12,12) ,Q (3,3) ,R (3,3)

[0137] Similarly, we can get c.

[0138] ●d

[0139]

[0140] ●e

[0141]

[0142] ●f

[0143]

[0144] d is calculated as follows:

[0145] variable:

[0146] Θ (4,4) ,Θ (4,7) ,Θ (10,4) ,Θ (10,7) ,Θ (7,4) ,Θ (10,10) ,Q (4,4) ,R (4,4)

[0147] Fixed parameters:

[0148]

[0149] addition:

[0150]

[0151] multiplication:

[0152]

[0153] addition:

[0154]

[0155] addition:

[0156]

[0157] addition:

[0158]

[0159] Replace the input with

[0160] Θ (5,5) ,Θ (5,8) ,Θ (11,5) ,Θ (11,8) ,Θ (8,5) ,Θ (11,11) ,Q (5,5) ,R (5,5)

[0161] Similarly, we can get e.

[0162] Replace the input with:

[0163] Θ (6,6) ,Θ (6,9) ,Θ (12,6) ,Θ (12,9) ,Θ (9,6) ,Θ (12,12) ,Q (6,6) ,R (6,6)

[0164] Similarly, we can get f.

[0165] ·x

[0166]

[0167] ·y

[0168]

[0169] ·z

[0170]

[0171] x is calculated as follows:

[0172] variable:

[0173] Θ (1,4) ,Θ (4,4) ,Θ (1,7) ,Θ (7,4) ,Θ (1,10) ,Θ (10,4) ,Θ (7,7) ,Θ (10,7) ,Θ (10,10) ,Q (1,4) ,R (1,4)

[0174] Fixed parameters:

[0175]

[0176] addition:

[0177]

[0178] Constant multiplication:

[0179]

[0180] addition:

[0181]

[0182] multiplication:

[0183]

[0184] addition:

[0185]

[0186] addition:

[0187]

[0188] addition:

[0189]

[0190] Replace the input with:

[0191] Θ (2,5) ,Θ (5,5) ,Θ (2,8) ,Θ (8,5) ,Θ (2,11) ,Θ (11,5) ,Θ (8,8) ,Θ (11,8) ,Θ (11,11) ,Q(2,5) ,R (2,5)

[0192] Similarly, we can get y.

[0193] Replace the input with:

[0194] Θ (3,6) ,Θ (6,6) ,Θ (3,9) ,Θ (9,6) ,Θ (3,12) ,Θ (12,6) ,Θ (9,9) ,Θ (12,9) ,Θ (12,12) ,Q (3,6) ,R (3,6)

[0195] The same logic can be used to obtain z.

[0196] Wherein step (2) is implemented as follows:

[0197] At each iteration, the prediction mean square error matrix H is measured k P k,k-1 H T +R k All of them are in the form mentioned in step (1). The inverse matrix form of the measurement prediction mean square error matrix obtained by Gaussian elimination method is as follows.

[0198] (H.P k,k-1 ·H T +R) -1

[0199]

[0200] First calculate ad-x 2 、be-y 2 ,cf-z 2 ;

[0201] Then calculate α / X, where X = ad-x 2 When α=a,d,x; when X=be-y 2 When α=b,e,y; when X=cf-z 2 When α=c,f,z;

[0202] Next, take the opposite of Y Get HP (k,k-1) H T +R's inverse matrix (HP (k,k-1) H T +R) -1 .

[0203] In calculating ad-x 2、be-y 2 ,cf-z 2 When , due to their similarity, parallel computing can be considered, which provides parallel conditions for hardware acceleration.

[0204] Similarly for α / X, that is, when X = ad-x 2 When α=a,d,x; when X=be-y 2 When α=b,e,y; when X=cf-z 2 When α=c,f,z, the calculation method is the same because they have the same form.

[0205] Wherein step (3) is implemented as follows:

[0206] In calculating P (k,k-1) After that, we can continue to calculate P (k,l-1) H T , of the following form:

[0207]

[0208] where Φ x,y is a 3*3 diagonal matrix; we can find that Φ x,y The elements on the diagonal of a diagonal matrix are expressed in a similar way to a, b, c, d, e, f, x, y, z, and the calculation method is similar. In actual operations, the division operation time is longer than the multiplication and addition operation time, so it is possible to consider calculating Φ in parallel with the inverse process. x,y .

[0209] Wherein step (4) is implemented as follows:

[0210] The matrix (HP) obtained in step (2) k,k-1 H T +R) -1 And the matrix P obtained in step (3) k,k-1 H T Perform matrix multiplication to obtain the Kalman gain K k .

[0211] Then use the obtained Kalman gain to update the state equation Estimate current status Using uncertainty to update the equation Update the current estimated uncertainty P k,k .

[0212] Repeat the above steps to continuously estimate the state of the system.

Claims

1. A method for fast calculation of digital Kalman gain of time-sharing uniform acceleration radar target, characterized in that: The steps include: Step 1: When tracking a target based on radar and using Kalman filtering to predict the trajectory of a target that is moving in time-sharing uniform acceleration in space, first determine the system state, establish the motion state extrapolation equation, state transfer matrix, and measurement matrix on the three degrees of freedom of the target space, and determine the measurement noise matrix and process noise matrix; For three degrees of freedom in space, the system state vector is determined as follows: in, represents the system state estimation vector at the kth moment, and a rectangular coordinate system is established with the center of the earth as the origin, where (x, y, z) represents the position of the observed target in three degrees of freedom, (v x ,v y ,v z ) represents the velocity of the observed target in three degrees of freedom, (a x ,a y ,a z ) represents the acceleration of the three degrees of freedom of the observed target, (j x ,j y ,j z ) represents the acceleration of the three degrees of freedom of the observed target; The extrapolation equation of the target motion state with uniform acceleration is shown in equation (2). The system state vector is converted from the estimated vector at the current time k to Extrapolation The prediction vector to the future time k+1 Where Δt represents the time step, They represent the estimated values ​​of the position (x, y, z) at the kth moment, respectively. represent the position at the kth moment (v x ,v y ,v z ), represent the position at the kth moment (a x ,a y ,a z ), represent the position at the kth moment (j x ,j y ,j z ), They represent (x, y, z), (v x ,v y ,v z )、(a x ,a y ,a z )、(j x ,j y ,j z )’s predicted value: The state transfer matrix is, where Δt represents the time step: Assuming that the observable state has position and velocity in three degrees of freedom, the measurement matrix is ​​as follows: In Kalman filtering, assuming that the process noise covariance matrix Q and the measurement noise covariance matrix R are constants, the noise in the Kalman filtering process is zero-mean Gaussian white noise, and the noises on the three degrees of freedom are independent of each other, then the measurement noise covariance matrix is ​​as follows, where r ab represents the measurement noise covariance of a and b, such as is the position x and velocity v in the x degree of freedom x Covariance of measurement noise: The process noise covariance matrix is ​​as follows, where σ ab represents the process noise covariance of a and b, such as is the position x and velocity v in the x degree of freedom x Covariance of process noise: Step 2: The measurement prediction mean square error matrix HP that needs to be inverted k,k-1 H T The form of +R is as follows: Where a, b, c, d, e, f, x, y, z are in the following form, and the value of each can be calculated from the elements in the state estimation mean square error matrix Θ, the process noise covariance matrix Q and the measurement covariance matrix at the k-1th moment; The form of Θ is as follows, where p ab Represents the state estimation covariance of a and b, such as p ab is the position x and velocity v in the x degree of freedom x The covariance of the state estimate, P k-1,k-1 is the state estimation covariance matrix of the k-1th step: The expressions of a, b, c, d, e, f, x, y, z are as follows, where Θ (x,y) represents the element at position (x,y) of the matrix Θ, and Δt represents the time step: ·a： ·b： ·c： ·d： ●e： ●f： ·x ·y： ·z： Step 3: Calculate the measurement prediction mean square error matrix H k P k,k-1 H T +R k The inverse matrix of The inverse matrix obtained by Gaussian elimination method is as follows: Step 4: P k,k-1 H T After derivation, the form is as follows: where Φ x,y It is a 3*3 diagonal matrix; Step 5: Substitute the matrix (HP) obtained in step 3 k,k-1 H T +R) -1 And the matrix P obtained in step 4 k,k-1 H T Perform matrix multiplication to obtain the Kalman gain K k ; Then use the obtained Kalman gain to update the state equation Estimate current status Using uncertainty to update the equation Update the current estimated uncertainty P k,k ; Step 6: Repeat Step 2-Step 5 to continuously estimate the state of the system.

2. A computer program, characterized in that The computer program enables a computer to execute the method as claimed in claim 1.

3. An electronic device, characterized in that: include: Processor and memory; The memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, so that the electronic device executes the method as claimed in claim 1.

4. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method as claimed in claim 1 is implemented.

5. A chip, characterized in that: include: A processor, used to call and run a computer program from a memory, so that a device equipped with the chip executes the method as claimed in claim 1.

6. A computer program product, characterized in that The computer program product comprises a computer storage medium storing a computer program, wherein the computer program comprises instructions executable by at least one processor, and when the instructions are executed by the at least one processor, the method according to claim 1 is implemented.

Images