Design method of adaptive unscented Kalman filter for photoelectric tracking system processing periodic motion targets
Through the design of adaptive unscented Kalman filter, the problems of large state estimation error and insufficient robustness of periodic motion targets in optoelectronic tracking system are solved, and high-precision filtering effect and good adaptability are achieved.
Patent Information
- Application Number
- CN202211553537.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-12-06
- Publication Date
- 2025-09-09
- Estimated Expiration
- 2042-12-06
AI Technical Summary
Existing filtering methods have problems such as large state estimation errors and insufficient robustness when dealing with periodic motion targets, especially when the motion parameters are not fixed and the motion is nonlinear and regular. In particular, in optoelectronic tracking systems, existing Kalman filters cannot effectively adapt to nonlinear periodic motion.
An adaptive unscented Kalman filter is designed. By setting the state equation and parameters, using Sigma points for state prediction and update, compensating model errors, it can achieve accurate estimation of nonlinear periodic motion targets and adapt to parameter changes in different motion forms.
It significantly improves the filtering accuracy, reduces the computational complexity, enhances the tolerance to model errors, and optimizes the estimation effect, especially for the state estimation and filtering of periodic motion targets in optoelectronic tracking systems.
Smart Images
Figure CN115859626B_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of signal filtering and estimation of optoelectronic tracking systems, and specifically relates to a method for designing an adaptive unscented Kalman filter for an optoelectronic tracking system to process periodic motion targets. The method is mainly used for state estimation and filtering when the tracked target performs periodic motion with a partially known motion function. The method effectively expands the robustness of the unscented Kalman filter, improves the filtering accuracy and the smoothness of the filtering curve, and optimizes the estimation effect of the filter. Background Art
[0002] The present invention can be applied to the field of photoelectric tracking systems for position estimation or prediction of periodically moving targets. In these applications, although the tracked target undergoes regular, nonlinear periodic motion, parameters such as the motion amplitude and frequency are not fixed, making it difficult to pre-establish the state equation of the tracked target ("Research on Filtering Technology for Ship Dynamic Positioning Systems" (Li Lanhua, Harbin Engineering University, 2006)). Currently, widely used filtering methods all use the kinematic characteristics of the tracked target, such as position, velocity, and acceleration, as the estimated state. When applied to predictive filtering of periodic moving targets, they are affected by sampling rate, computational complexity, and model uncertainty, resulting in large state estimation errors ("Modeling and State Estimation of Multi-Rate Sampled Data Systems" (Jiang Hongxia, Jiangnan University, 2008)). Specifically, the widely used Kalman filter has optimal filtering characteristics for Gaussian white noise, but it requires a precise state equation and is limited to linear motion scenarios. When faced with nonlinear, periodic moving targets with imprecise state equation parameters, its filtering performance deteriorates significantly ("Applied optimal estimation" (Arthur G. Proceedings of the IEEE, 1974)). Common robust filters can compensate for the uncertainty in the motion model of the tracked target, improving the accuracy of position estimation or prediction to a certain extent. However, their robustness is limited and requires that the parameter variations of periodically moving targets do not exceed a certain range (A Framework for State-Space Estimation with Uncertain Models, Ali H. Sayed, IEEE TRANSACTIONS ON AUTOMATIC CONTROL, 2001). Classical unscented Kalman filters and cubic Kalman filters also suffer from this shortcoming. (Kalman Filter Principles and Applications, Huang Xiaoping, 2015) and Adaptive Robust CKF Integrated Navigation Algorithm Based on Chi-Square Test, Xiong Xin, Huang Guoyong, and Wang Xiaodong, Journal of Detection and Control, 2019) While these filtering methods can improve filtering accuracy for nonlinearly moving targets to a certain extent, they still require high precision for the estimated target's motion model, limiting their universality. As filter performance requirements continue to increase in fields such as optoelectronic tracking, designing nonlinear filters with good adaptability becomes particularly important when the estimated target exhibits regular periodic motion. We urgently need to improve the existing filtering methods so that they can still maintain good filtering performance in digital control systems with sampling period limitations. Summary of the Invention
[0003] Aiming at the filter running in the digital control system limited by the sampling period, when the tracking object of the photoelectric tracking system performs periodic motion with a certain regularity, the present invention proposes an adaptive unscented Kalman filter design method for the photoelectric tracking system to process periodic motion targets.
[0004] In order to achieve the purpose of the present invention, an adaptive unscented Kalman filter design method for an optoelectronic tracking system to process periodic motion targets is provided. The present invention first clarifies the state equation and the meaning of the parameters therein:
[0005]
[0006] In this state equation, 、 are the estimated state and observation value of the periodic motion tracked target respectively; the estimated initial value of the state of the periodic motion tracked target , state transition noise , observation noise Conforms to Gaussian distribution; 、 are the state transfer equation and the observation equation respectively.
[0007] is the Kronecker symbolic equation, that is hour, ; hour, . 、 、 satisfy:
[0008]
[0009] The present invention uses the key parameters in the filtered target motion equation and the time variable of the motion equation as the estimated value. , its status includes the current time ,cycle (or get the angular frequency linearly related to it parameters) and state transfer equations and the observation equation Key parameters in :
[0010] .
[0011] When using the filter of the present invention, the following steps should be followed:
[0012] Step (1): Set the parameters used for UT transformation in the Unscented Kalman Filter (UKF) , set the initial value of the UKF state estimate , in this initial value ,parameter It should be selected according to the maximum likelihood value of the motion model of the tracked target; set the initial value of the state posterior covariance , observation noise covariance , process noise covariance , are selected according to the actual application scenario; set the state transfer equation of the unscented Kalman filter and the observation equation , where the state transfer equation The form is fixed, and the following formula is the sampling period, The value growth rate is fixed, and the parameters of the filtered target motion equation Basically unchanged:
[0013]
[0014] Observation equation Select according to the actual motion trajectory of the tracked target. For example, the observation equation when the tracked target is doing sinusoidal motion Observation equation when performing triangular motion with the tracked target They are as follows:
[0015] .
[0016] in, in represent the amplitude and angular frequency of the sinusoidal motion respectively ( ); in Represent the amplitude and angular frequency of triangular motion respectively ( ), the proportion of the rising section of the triangle wave.
[0017] Step (2): According to the preset The value and estimated state Dimension n calculation parameters :
[0018]
[0019] Step (3): According to the estimated state and its dimension n, preset , calculated in the previous step , state posterior covariance , 2n+1 Sigma points are calculated ;
[0020] ,
[0021] in, represents the square root of the i-th column of the matrix M.
[0022] Step (4): Calculate 2n+1 Sigma points Their respective weights :
[0023] .
[0024] Step (5): 2n+1 Sigma points According to the state transfer equation , and make a one-step prediction to get :
[0025] .
[0026] Step (6): According to 2n+1 Points and corresponding weights , calculate the estimated state The prior state value of and the state prior covariance :
[0027]
[0028] .
[0029] Step (7): Prior state value Perform UT transformation to obtain 2n+1 new Sigma points :
[0030] .
[0031] Step (8): 2n+1 Sigma points According to the observation equation , respectively, and make observation simulations to obtain :
[0032] .
[0033] Step (9): According to 2n+1 Points and corresponding weights , predict the observed value and covariance and :
[0034] .
[0035] Step (10): Calculate the Kalman gain matrix :
[0036]
[0037] Step (11): Calculate the posterior state estimate of the system and covariance .
[0038]
[0039] Step (12): Return to step (3) to perform a new round of filtering calculation.
[0040] Compared with the prior art, the present invention has the following advantages:
[0041] (1) Compared with the traditional Kalman filtering method, the present invention can be applied to tracked nonlinear periodic motion targets, and can achieve a significant improvement in filtering accuracy with less additional computational complexity;
[0042] (2) Compared with traditional robust filtering methods, the adaptive unscented Kalman filter in the present invention has greater tolerance to model errors and can achieve accurate estimation of nonlinear periodic motion targets by directly estimating model parameters;
[0043] (3) Compared with the traditional unscented Kalman filter method, the present invention does not rely on the accurate target motion trajectory equation and shows excellent adaptability to the estimated targets with similar motion forms but different key parameters;
[0044] (4) Compared with common filtering methods, the present invention can effectively compensate for the state equation discretization error caused by the sampling time and operation time of the control system, and significantly improve the filtering accuracy. BRIEF DESCRIPTION OF THE DRAWINGS
[0045] Figure 1 This is a comparison chart of the simulation results of the classic Kalman filter and the traditional unscented Kalman filter, where (a) is the trajectory simulation curve and the filtering curves of the two filters, and (b) is the cumulative estimation error of the two filters statistically analyzed by the present invention.
[0046] Figure 2 This is a comparison chart of the simulation results of the classic Kalman filter and the adaptive unscented Kalman filter, where (a) is the trajectory simulation curve and the filtering curves of the two filters, and (b) is the cumulative estimation error of the two filters calculated by the present invention.
[0047] Figure 3 This is a comparison chart of the simulation results of the real trajectory, measured trajectory, classic Kalman filter trajectory and adaptive unscented Kalman filter trajectory when the tracked target performs variable elliptical motion (both the x and y axes perform variable sinusoidal motion).
[0048] Figure 4 This is a comparison chart of the cumulative estimation errors of the classic Kalman filter and the adaptive unscented Kalman filter.
[0049] Figure 5 is a comparison diagram of the target motion equation parameters estimated by the adaptive unscented Kalman filter and the true target motion equation parameters, where (a) is the comparison between the estimated value of the motion parameter of the estimated target on the x-axis and the true value, and (b) is the comparison between the estimated value of the motion parameter of the estimated target on the y-axis and the true value. DETAILED DESCRIPTION
[0050] The present invention is further described below with reference to the accompanying drawings and specific embodiments.
[0051] To achieve the purpose of the present invention, the present invention provides an adaptive unscented Kalman filter design method for an optoelectronic tracking system to process periodic motion targets. The algorithm flow is as follows:
[0052] Step (1): Set the parameters used by the unscented Kalman filter , set the initial state estimate of the UKF periodic motion target to be tracked , initial value of state posterior covariance , observation noise covariance , process noise covariance ; And according to the principles mentioned above, the movement time of the filtered target , motion period T (or a linearly related value such as angular frequency ), key parameters of motion model As the estimated state, the traditional state transfer equation and observation equation are further converted into a new state transfer equation and the observation equation ;
[0053] Step (2): Calculate parameters ;
[0054] Step (3): Calculate 2n+1 Sigma points ;
[0055] Step (4): Calculate 2n+1 Sigma points Their respective weights ;
[0056] Step (5): Make a one-step prediction to get ;
[0057] Step (6): Calculate the estimated state The prior state value of and the state prior covariance ;
[0058] Step (7): Get 2n+1 new Sigma points ;
[0059] Step (8): Do observation simulation to get ;
[0060] Step (9): Predict observations and covariance and ;
[0061] Step (10): Calculate the Kalman gain matrix ;
[0062] Step (11): Calculate the posterior state estimate of the system and covariance ;
[0063] Step (12): Return to step (3) to perform a new round of filtering calculation.
[0064] The adaptive unscented Kalman filter obtained through the above steps significantly improves the filtering accuracy when the filtering target undergoes periodic motion and the motion equation is not completely known.
[0065] The following uses the classic Kalman filter, the traditional unscented Kalman filter, and the adaptive unscented Kalman filter to simulate the filtering effect under the conditions of variable sinusoidal motion and variable elliptical motion (both the x and y axes perform variable sinusoidal motion) of the tracking target of the photoelectric tracking system, and describes in detail the design process and effect of the present invention:
[0066] (1): The relevant formulas and parameters used in the simulation of variable sinusoidal motion are as follows:
[0067] Trajectory simulation related formulas and parameters:
[0068]
[0069] Classic Kalman filter related formulas and parameters:
[0070]
[0071] Traditional unscented Kalman filter related formulas and parameters:
[0072]
[0073] Adaptive unscented Kalman filter related formulas and parameters:
[0074]
[0075] in, The covariances are Gaussian white noise, Representative vector The kth dimension of
[0076] (2): Simulate the adaptive unscented Kalman filter according to the method described in steps (2) to (12), and simulate the classical Kalman filter and the traditional unscented Kalman filter and obtain data; a total of 200 steps of trajectory generation and filtering simulation are performed.
[0077] (3): Figure 1 Comparison of simulation results for the classic Kalman filter and the traditional unscented Kalman filter. (a) shows the trajectory simulation curve and the filtering curves for the two filters. The left subfigure does not clearly show the comparison of the two filters' performance, so in (b), the present invention calculates the cumulative estimation error of the two filters. The figure shows that the adaptive unscented Kalman filter (UKF) achieves better estimation accuracy than the classic Kalman filter (KF), but the advantage is limited.
[0078] (4): Figure 2 This is a comparison chart of the simulation results of the classic Kalman filter and the adaptive unscented Kalman filter. (a) is the trajectory simulation curve and the filtering curves of the two filters. It can be seen that the estimation accuracy of the adaptive unscented Kalman filter is significantly higher than that of the classic Kalman filter. In order to more intuitively show the effect comparison of the two filters, in (b), the present invention counted the cumulative estimation errors of the two filters. It can be seen that the estimation accuracy of the adaptive unscented Kalman filter (UKF) is significantly higher than that of the classic Kalman filter (KF), and the estimation error is reduced by about 35%. Figure 1 Compared with the results, the estimation accuracy of the adaptive unscented Kalman filter is also significantly better than that of the traditional unscented Kalman filter, which proves the improvement of the adaptive unscented Kalman filter in filtering accuracy.
[0079] (5): For example Figure 3 This chart compares the simulation results of the true trajectory, the measured trajectory, the classic Kalman filter trajectory, and the adaptive unscented Kalman filter trajectory when the tracked target performs variable elliptical motion (variable sinusoidal motion on both the x and y axes). It can be seen that both filters improve position estimation accuracy, but the adaptive unscented Kalman filter achieves significantly better results, with the filtered value closer to the true motion trajectory.
[0080] (6): In order to better demonstrate the advantages of the adaptive unscented Kalman filter when the tracked target is performing variable elliptical motion (both the x and y axes are performing variable sinusoidal motion), Figure 4 By comparing the cumulative estimation errors of the classic Kalman filter and the adaptive unscented Kalman filter, it can be seen that the estimation accuracy of the adaptive unscented Kalman filter (UKF) is significantly improved compared with the classic Kalman filter (KF), which once again proves the advantage of the adaptive unscented Kalman filter in filtering accuracy.
[0081] (7): Figure 5 shows the target motion equation parameters estimated by the adaptive unscented Kalman filter and the actual target motion equation parameters. (a) compares the estimated parameters and the actual parameters of the target in the x-axis direction, and (b) compares the estimated parameters and the actual parameters of the target in the y-axis direction. Figure 5 shows that the estimated results of the adaptive unscented Kalman filter are very close to the actual target motion equation parameters, further demonstrating the filtering capability and effectiveness of the present invention.
Claims
1. A design method for an adaptive unscented Kalman filter for an optoelectronic tracking system to process periodic motion targets, characterized by: An adaptive unscented Kalman filter is used for photoelectric tracking systems to process periodic motion targets. When the estimated target undergoes periodic motion for which the equation is partially known, the accuracy and smoothness of the unscented Kalman filter's filtering results are significantly improved by estimating the intrinsic parameters of the motion function rather than the external physical manifestations of the moving target, including position and velocity. This enhances the filter's adaptability. In addition, the greater the restrictions on system sampling time or calculation time, the greater the relative advantages of the adaptive unscented Kalman filter algorithm. The specific implementation steps are as follows: Step (1): Set the parameters used for UT transformation in the unscented Kalman filter UKF , set the initial state estimate of the UKF periodic motion target to be tracked , initial value of state posterior covariance , observation noise covariance , process noise covariance , the state transition equation of the unscented Kalman filter and the observation equation ; Step (2): According to the preset The value and estimated state Dimension n calculation parameters ; Step (3): According to the estimated state and its dimension n, preset , calculated in the previous step , state posterior covariance , 2n+1 Sigma points are calculated ; Step (4): Calculate 2n+1 Sigma points Their respective weights ; Step (5): 2n+1 Sigma points According to the state transfer equation , and make a one-step prediction to get ; Step (6): According to 2n+1 Points and corresponding weights , calculate the estimated state The prior state value of and the state prior covariance ; Step (7): Prior state value Perform UT transformation to obtain 2n+1 new Sigma points ; Step (8): 2n+1 Sigma points According to the observation equation , respectively, and make observation simulations to obtain ; Step (9): According to 2n+1 Points and corresponding weights , predict the observed value and covariance and ; Step (10): Calculate the Kalman gain matrix ; Step (11): Calculate the posterior state estimate of the system and covariance ; Step (12): Return to step (3) to perform a new round of filtering calculation.
2. The method for designing an adaptive unscented Kalman filter for an optoelectronic tracking system to process periodic motion targets according to claim 1, characterized in that: The status includes the current time ,cycle and the state transition equation and the observation equation Key parameters in : 。 3. The method for designing an adaptive unscented Kalman filter for an optoelectronic tracking system to process periodic motion targets according to claim 1, wherein: middle This is the sampling period of the control system. This parameter is a fixed value: in, is the sampling period.
4. The method for designing an adaptive unscented Kalman filter for an optoelectronic tracking system to process periodic motion targets according to claim 2, wherein: Estimated state value All dimensions, current time It is a fixed-step growth cycle and key parameters is a constant value.
5. The method for designing an adaptive unscented Kalman filter for an optoelectronic tracking system to process periodic motion targets according to claim 2, wherein: For the estimated state value , when the filtered target is in periodic motion, There is an estimated value of The deviation of the motion cycle does not affect the estimation effect.
6. The method for designing an adaptive unscented Kalman filter for an optoelectronic tracking system to process periodic motion targets according to claim 2, wherein: For the state transfer equation and the observation equation In the traditional unscented Kalman filter, the estimated quantity is composed of the kinematic feature position and velocity of the estimated target. , is the kinematic equation of the estimated target, To extract the estimated The position of the target in several dimensions; using the parameters of the kinematic equations of the estimated target and the current moment Composition of the estimated quantity ,Converting the traditional estimation of variable position and velocity values into estimation of constant quantities or quantities with a certain change process can effectively improve the adaptability of the unscented Kalman filter.