A Fast Calculation Method for Digital Kalman Gain of Radar Targets under Time-Division Uniform Acceleration

Abstract

This invention discloses a fast method for calculating the digital Kalman gain of a time-division uniformly accelerated radar target. By extracting features from the prediction and update equations in the Kalman filter, the method further couples the Kalman gain calculation formula at time k with the state one-step prediction covariance equation at time k-1. This is achieved by pre-calculating the HP matrix. k,k‑1 H T +R k This method achieves optimization results by utilizing the matrix HP. k,k‑1 H T +R k By leveraging the mathematical properties of the Kalman filter, and deeply exploring the distribution patterns of its inverse matrix elements, this invention enables rapid computation of the inverse matrix. This improvement significantly enhances the computation speed of the Kalman gain. Compared to traditional Kalman filtering algorithms, this invention demonstrates greater potential in parallel computing, effectively reducing computational complexity and significantly accelerating the execution speed of the Kalman filtering process. This invention provides a more efficient solution for real-time tracking and prediction of spatial targets, offering significant advantages, especially in scenarios with limited computing resources or requiring high-frequency updates.

Description

Technical Field

[0001] This invention belongs to the field of radar technology, specifically relating to a method for fast calculation of digital Kalman gain of radar targets with time-division uniform acceleration. Background Technology

[0002] Kalman filtering is a recursive algorithm used to estimate the state of dynamic systems, especially those containing noise or uncertainty. This algorithm is well-suited for target tracking and trajectory prediction because it can optimize the estimation 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 the noise in sensor data, leading to inaccurate measurements. Kalman filtering, through noise modeling and recursive estimation, can provide a more accurate estimate of the target's true state. In trajectory prediction problems, Kalman filters can predict a target's trajectory over a future period based on past motion states and current observations. It does this by recursively updating the target's state (such as position and velocity) and predicting its state at the next moment or even further into the future based on a motion model.

[0004] Kalman filtering requires updating the Kalman gain matrix in each cycle, which is a key factor in the state estimation process. However, a common challenge is the high computational cost of the Kalman gain matrix, especially in high-dimensional scenarios. This is because the update steps of the covariance matrix and Kalman gain in Kalman filtering involve matrix operations (including matrix inversion), which can lead to computational inefficiency at high dimensions. The calculation of the Kalman gain involves matrix inversion, which typically has a computational complexity of O(n^2). 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 computation time will increase dramatically, resulting in lower efficiency for systems with high real-time requirements.

[0005] Because the Kalman gain calculation requires inverting the matrix HP... k,k-1 H T +R kSince it is a symmetric positive definite matrix, it can be solved using matrix decomposition. In Kalman filtering, the symmetric positive definite matrix can be solved using Cholesky decomposition, an efficient method for decomposing symmetric positive definite matrices. This method has high numerical stability and is faster than direct inversion, but in large-scale, high-dimensional systems, it may still not meet real-time requirements, and it cannot complete the decomposition when the values ​​are unstable. Extended Kalman filtering is a nonlinear extension of Kalman filtering used to handle nonlinear systems. Although it was not specifically designed to solve the computational complexity problem of the Kalman gain matrix, linearization reduces the computational complexity of the gain matrix in many practical applications. Extended Kalman filtering requires linearization in each iteration, which may affect the accuracy of the calculation, especially for high-dimensional or highly nonlinear systems, where the matrix operation is still relatively large. In some complex high-dimensional systems, the state transition matrix or observation matrix may be sparse. Using sparse matrix techniques can significantly reduce the computational cost because only a few elements in a sparse matrix are non-zero, significantly reducing the complexity of matrix operations. This method is only suitable for situations where the dynamic characteristics of the system change relatively locally. Summary of the Invention

[0006] To overcome the shortcomings of existing technologies, this invention provides a fast digital Kalman gain calculation method for time-division uniformly accelerated radar targets. By extracting features from the prediction and update equations in the Kalman filter, the Kalman gain calculation formula at time k is further coupled with the state one-step prediction covariance equation at time k-1. This is achieved by pre-calculating the HP matrix. k,k-1 H T +R k This method achieves optimization results by utilizing the matrix HP. k,k-1 H T +R k By leveraging the mathematical properties of the Kalman filter, and deeply exploring the distribution patterns of its inverse matrix elements, this invention enables rapid computation of the inverse matrix. This improvement significantly enhances the computation speed of the Kalman gain. Compared to traditional Kalman filtering algorithms, this invention demonstrates greater potential in parallel computing, effectively reducing computational complexity and significantly accelerating the execution speed of the Kalman filtering process. This invention provides a more efficient solution for real-time tracking and prediction of spatial targets, offering significant advantages, especially in scenarios with limited computing resources or requiring high-frequency updates.

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

[0008] Step 1: When tracking targets based on radar and predicting the trajectory of targets undergoing time-division uniform acceleration in space using Kalman filtering, first determine the system state, establish the extrapolation equation of the motion state on the three degrees of freedom of the target space, the state transition matrix, the measurement matrix, and determine the measurement noise matrix and the process noise matrix.

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

[0010]

[0011] in, Let represent the system state estimation vector at time k. A rectangular coordinate system is established with the Earth's center as the origin, where (x, y, z) represents the position of the observed target in three degrees of freedom, and (v... x ,v y ,v z (a) represents the velocity of the observed target in three degrees of freedom. x ,a y ,a z (j) represents the acceleration of the observed target in three degrees of freedom. x ,j y ,j z () represents the jerk of the observed target in three degrees of freedom;

[0012] The extrapolation equation for the target motion state of time-sharing uniform acceleration is shown in equation (2), which transforms the system state vector from the estimated vector at the current time k. extrapolation Prediction vector up to future time k+1 Where Δt represents the time step. Let x, y, and z represent the estimated values ​​of the position (x, y, z) at time k. These represent the positions at time k (v) x ,v y ,v z The estimated value of ) These represent the positions at time k (a, b, c, ..., d) respectively. x ,a y ,a z The estimated value of ) These represent the positions at time k (j) and k respectively. x ,j y ,j z The estimated value of ) Let (x, y, z) and (v) represent the time interval at time k+1, respectively. x ,v y ,v z ), (a x ,a y,a z (j) x ,j y ,j z Predicted value:

[0013]

[0014] The state transition matrix is ​​given by , where Δt represents the time step:

[0015]

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

[0017]

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

[0019]

[0020] The process noise covariance matrix is ​​as follows, where σ ab Representing the process noise covariance of a and b, for example It 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 needs to be inverted. k,k-1 H T The +R form is as follows:

[0023]

[0024] The forms of a, b, c, d, e, f, x, y, z are as follows, and the value of each can be calculated from the elements in the mean square error matrix Θ of the state estimation at time k-1, the process noise covariance matrix Q, and the measurement covariance matrix.

[0025] The form of Θ is as follows, where p ab This represents the state estimate covariance of a and b, for example, p. ab It 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 Here is the state estimation covariance matrix for the (k-1)th step:

[0026]

[0027] The expressions for a, b, x, d, e, f, x, y, z are as follows, where Θ (x,y) Let θ represent the element at position (x, y) in matrix Θ, and Δt represent 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 mean square error matrix H of measurement prediction k P k,k-1 H T +R k The inverse matrix;

[0048] The inverse matrix is ​​obtained by using Gaussian elimination 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 3x3 diagonal matrix;

[0053] Step 5: Convert the matrix (HP) obtained in Step 3 into... 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, using the obtained Kalman gain, the state update equation is applied. Estimate the current state Using uncertainty update equations Update the current estimated uncertainty P k,k .

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

[0056] A computer program that causes a computer to perform the above-described fast calculation method for digital Kalman gain.

[0057] An electronic device includes: 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 to enable the electronic device to perform the above-described fast digital Kalman gain calculation method.

[0058] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-described method for fast calculation of digital Kalman gain.

[0059] A chip includes a processor for retrieving and running a computer program from memory, causing a device equipped with the chip to perform the aforementioned fast digital Kalman gain calculation method.

[0060] A computer program product includes a computer storage medium storing a computer program, the computer program including instructions executable by at least one processor, which, when executed by the at least one processor, implement the above-described fast calculation method for digital Kalman gain.

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

[0062] This invention provides a fast calculation method for digital Kalman filtering. Based on mathematical derivation, it simplifies the calculation and realizes a calculation channel from the covariance state estimation mean square matrix at time k-1 directly to the Kalman gain matrix. It omits the storage and calculation of intermediate results, reduces computational complexity, and speeds up the Kalman filtering process, thus making it more suitable for systems with high real-time requirements. Attached Figure Description

[0063] Figure 1 This is a flowchart of the method framework of the present invention. Detailed Implementation

[0064] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0065] In radar target tracking systems, Kalman filtering is often used for data fusion and trajectory prediction. When predicting the trajectory, treating the tracked target as undergoing uniformly accelerated motion can improve the accuracy of the prediction, but it also increases the data dimensionality and computational complexity, especially in calculating the Kalman gain. To reduce the computational complexity of the Kalman gain and improve the concurrency of Kalman gain calculation, this invention proposes a fast Kalman gain calculation method by combining advance calculation with the distribution characteristics of matrix elements. By further coupling the covariance state estimation mean square matrix at time k-1 with the Kalman gain calculation equation, and after mathematical derivation and proof, simplified calculations are performed to quickly obtain the inverse matrix of the measurement prediction mean square error matrix, thereby reducing the computational load and accelerating the Kalman gain calculation process.

[0066] In the field of radar target tracking, accurate estimation of target position, velocity, acceleration, and jerk is crucial for the stability and security of systems such as air defense early warning and navigation guidance. However, radar signals are often subject to noise interference, resulting in high uncertainty in observation data and increasing the difficulty of target state estimation. To achieve high-precision target tracking, the system needs a technique capable of extracting reliable state information from noise.

[0067] Kalman filtering, due to its superior recursive estimation and noise suppression capabilities, has become a core algorithm in radar target tracking. Kalman filtering utilizes prior information about the target state and current observation data, continuously refining the target's estimate through a cyclical process of prediction and updating. It can optimize real-time target state estimation in high-noise environments, resulting in more accurate and stable tracking. In engineering, the application of Kalman filtering has significantly improved the performance of radar systems, enabling them to accurately track fast-moving targets in dynamic environments.

[0068] When tracking targets based on radar, and using Kalman filtering to predict the trajectory of a target undergoing time-division uniform acceleration in space, it is necessary to first determine the system state, establish the extrapolation equation of the motion state on the three degrees of freedom of the target space, the state transition 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-division uniformly accelerated target in space, it is necessary to first determine the system state, establish the extrapolation equations of the motion state on the three degrees of freedom of the target space, the state transition 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 three degrees of freedom x, y, z in space, the system state vector is represented as follows:

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

[0072]

[0073] Let represent the system state estimation vector at time k. A rectangular coordinate system is established with the Earth's center as the origin, where (x, y, z) represents the position of the observed target in three degrees of freedom, and (v... x ,v y ,v z (a) represents the velocity of the observed target in three degrees of freedom. x ,a y ,a z (j) represents the acceleration of the observed target in three degrees of freedom. x ,j y ,j z () represents the jerk of the observed target in three degrees of freedom;

[0074] The extrapolation equation for the target motion state of time-sharing uniform acceleration is shown in equation (2), which transforms the system state vector from the estimated vector at the current time k. extrapolation Prediction vector up to future time k+1 Where Δt represents the time step. Let x, y, and z represent the estimated values ​​of the position (x, y, z) at time k. These represent the positions at time k (v) x ,v y ,v z The estimated value of ) These represent the positions at time k (a, b, c, ..., d) respectively. x ,a y ,a z The estimated value of ) These represent the positions at time k (j) and k respectively. x ,j y ,j z The estimated value of ) Let (x, y, z) and (v) represent the time interval at time k+1, respectively. x ,v y ,v z ), (a x ,a y ,a z (j) x ,j y ,j z Predicted value:

[0075]

[0076] The system state transitions from time k to time k+1(X) k+1,k =FX k,k The state transition matrix F is as follows, where Δt represents the time step:

[0077]

[0078] The observable state of the UAV has three degrees of freedom: position and velocity. Therefore, the measurement matrix is:

[0079]

[0080] The noise generated by the Kalman filter is all zero-mean Gaussian white noise, and the noise in the three degrees of freedom is independent of each other. Furthermore, the process noise and the measured noise are uncorrelated, satisfying the following conditions:

[0081] The noise has a mean of zero, i.e., E[n(t)] = 0.

[0082] The noise distribution follows a Gaussian (normal) distribution, usually represented by the standard deviation σ: n(t)~

[0083] N(0,σ 2 ).

[0084] Wherein, variance σ 2The intensity of the noise is described.

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

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

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

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

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

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

[0091] When the three degrees of freedom noises are independent of each other, the measurement noise covariance on any two degrees of freedom is 0, i.e., r xy =0, Therefore, the measurement noise covariance matrix can be simplified as follows, where r ab The measurement noise covariance of a and b, for example It is the position x and velocity v in the x-degree of freedom. x Covariance of the measured noise:

[0092]

[0093] When the three degrees of freedom noises are independent of each other, the process noise covariance on any two degrees of freedom is 0, i.e., σ xy =0, σ xz =0, Therefore, the process noise covariance matrix can be simplified as follows, where σ ab Representing the process noise covariance of a and b, for example It is the position x and velocity v in the x-degree of freedom. x Covariance of process noise:

[0094]

[0095] In practical applications, a single filtering process is only a sample of the overall random process. Moreover, the true value of the initial filtering state is often unknown. Therefore, the initial filtering state is usually set to a value near the true value, or sometimes even directly set to the zero vector. As a result, the estimation results of Kalman filtering are always biased in practice. However, 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 UAV state vector is:

[0097]

[0098] The initial covariance matrix is:

[0099]

[0100] The following content extrapolates from time k-1 to time k, setting the mean square error matrix P for the state estimation at time k-1. k-1,k-1 For, where p ab Representing the state covariance of a and b, for example It is the position x and velocity v in the x-degree of freedom. x Covariance of state estimation:

[0101]

[0102] Since the noise on any two degrees of freedom is independent of each other, the noise covariance of any two degrees of freedom is zero.

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

[0104] Using the conditions established above, and through mathematical derivation, it can be determined that the measurement prediction mean square error matrix HP, which needs to be inverted, is... k,k-1 H T The +R form is shown below:

[0105]

[0106] The forms of a, b, c, d, e, f, x, y, z are as follows, and the value of each can be calculated from the elements of the mean square error matrix Θ of the state estimation at time k, the process noise covariance matrix Q, and the measurement noise covariance matrix; where Θ has the following form:

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

[0108]

[0109] The forms and calculation methods of a, b, c, d, e, f, x, y, z in the measurement prediction mean square error matrix are as follows, where Θ (x,y) Let θ represent the element at position (x, y) in matrix Θ, and Δt represent 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 obtain 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 obtain 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 obtain 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 obtain 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 obtain 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] Similarly, we can obtain z.

[0196] Step (2) is implemented as follows:

[0197] In each iteration, the mean square error matrix H of the measurement prediction is... k P k,k-1 H T +R k All are in the form mentioned in step (1). The inverse matrix form of the measurement prediction mean square error matrix obtained by the 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; where X = be - y 2 When α = b, e, y; where X = cf - z 2 When α = c, f, z;

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

[0203] In calculating ad-x 2be-y 2 cf-z 2 Since they are similar, parallel computing can be considered, which provides the conditions for hardware acceleration.

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

[0205] Step (3) is implemented as follows:

[0206] After calculating P (k,k-1) Then, P can be calculated. (k,l-1) H T The format is as follows:

[0207]

[0208] Where Φ x,y It is a 3x3 diagonal matrix; we can find Φ x,y The elements on the diagonal of a diagonal matrix are represented similarly to a, b, c, d, e, f, x, y, z, and the calculation methods are similar. In practical operations, division takes longer than multiplication and addition, so it's possible to calculate Φ in parallel with the inversion process. x,y .

[0209] 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, using the obtained Kalman gain, the state update equation is applied. Estimate the current state Using uncertainty update equations 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 the digital Kalman gain of a time-division uniformly accelerated radar target, characterized in that, Includes the following steps: Step 1: When tracking targets based on radar and predicting the trajectory of targets undergoing time-division uniform acceleration in space using Kalman filtering, first determine the system state, establish the extrapolation equation of the motion state on the three degrees of freedom of the target space, the state transition matrix, the measurement matrix, and determine the measurement noise matrix and the process noise matrix. For a system with three degrees of freedom in space, the state vector is determined as follows: in, Let represent the system state estimation vector at time k. A rectangular coordinate system is established with the Earth's center as the origin, where (x, y, z) represents the position of the observed target in three degrees of freedom, and (v... x ,v y ,v z (a) represents the velocity of the observed target in three degrees of freedom. x ,a y ,a z (j) represents the acceleration of the observed target in three degrees of freedom. x ,j y ,j z () represents the jerk of the observed target in three degrees of freedom; The extrapolation equation for the target motion state of time-sharing uniform acceleration is shown in equation (2), which transforms the system state vector from the estimated vector at the current time k. extrapolate the prediction vector to the future time k+1 in Δt represents the time step. Let x, y, and z represent the estimated values ​​of the position (x, y, z) at time k. Let v and v represent the velocity at time k, respectively. x ,v y ,v z The estimated value of ) Let a and b represent the acceleration at time k, respectively. x ,a y ,a z The estimated value of ) Let j represent the jerk at time k, respectively. x ,j y ,j z The estimated value of ) Let (x, y, z) and (v) represent the time interval at time k+1, respectively. x ,v y ,v z ), (a x ,a y ,a z (j) x ,j y ,j z Predicted value: The state transition matrix is ​​as follows, where Δt represents the time step: Assuming the observable state has three degrees of freedom: position and velocity, the measurement matrix is ​​as follows: In Kalman filtering, assuming the process noise covariance matrix Q and the measurement noise covariance matrix R are constants, and the noise in the Kalman filtering process is all zero-mean Gaussian white noise, and the noise in the three degrees of freedom is independent of each other, then the measurement noise covariance matrix is ​​as follows, where r ab The measurement noise covariance of a and b, for example It 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 Representing the process noise covariance of a and b, for example It 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 needs to be inverted. k,k-1 H T The +R form is as follows: The forms of a, b, c, d, e, f, x, y, z are as follows, and the value of each can be calculated from the elements in the mean square error matrix Θ of the state estimation at time k-1, the process noise covariance matrix Q, and the measurement covariance matrix. The form of Θ is as follows, where p ab This represents the covariance of the state estimates of a and b, for example... It 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 Here is the state estimation covariance matrix for the (k-1)th step: The expressions for a, b, c, d, e, f, x, y, z are as follows, where Θ (x,y) Let θ represent the element at position (x, y) in matrix Θ, and Δt represent the time step. ·a： ·b： ·c： ·d： ·e： ·f： ·x： ·y： ·z： Step 3: Calculate the measurement prediction mean square error matrix HP k,k-1 H T +R inverse matrix; The inverse matrix is ​​obtained by using Gaussian elimination as follows: Step 4: P k,k-1 H T After derivation, the form is as follows: Where Φ x,y It is a 3x3 diagonal matrix; Step 5: Convert the matrix (HP) obtained in Step 3 into... 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, using the obtained Kalman gain, the state update equation is applied. Estimate the current state Using uncertainty update equations Update the current estimated uncertainty P k,k ; Step 6: Repeat steps 2-5 to continuously estimate the state of the system.

2. 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 to cause the electronic device to perform the method as described in claim 1.

3. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the method as described in claim 1.

4. A chip, characterized in that, include: A processor for retrieving and running a computer program from memory, causing a device on which the chip is mounted to perform the method as described in claim 1.

5. A computer program product, characterized in that, The computer program product includes a computer storage medium storing a computer program, the computer program including instructions executable by at least one processor, which, when executed by the at least one processor, implement the method as described in claim 1.

Images