CN117455960A - Passive positioning filtering algorithm for airborne photoelectric system to ground under time-varying observation noise condition - Google Patents
Passive positioning filtering algorithm for airborne photoelectric system to ground under time-varying observation noise condition Download PDFInfo
- Publication number
- CN117455960A CN117455960A CN202311780923.9A CN202311780923A CN117455960A CN 117455960 A CN117455960 A CN 117455960A CN 202311780923 A CN202311780923 A CN 202311780923A CN 117455960 A CN117455960 A CN 117455960A
- Authority
- CN
- China
- Prior art keywords
- target point
- moment
- observation
- coordinate system
- time
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 50
- 238000001914 filtration Methods 0.000 title claims abstract description 23
- 239000011159 matrix material Substances 0.000 claims abstract description 54
- 238000000034 method Methods 0.000 claims description 39
- 230000008569 process Effects 0.000 claims description 17
- 230000009466 transformation Effects 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 9
- 230000007704 transition Effects 0.000 claims description 7
- 230000005693 optoelectronics Effects 0.000 claims description 4
- 230000003321 amplification Effects 0.000 claims description 3
- 238000003199 nucleic acid amplification method Methods 0.000 claims description 3
- 101100533306 Mus musculus Setx gene Proteins 0.000 claims description 2
- 238000005259 measurement Methods 0.000 abstract description 15
- 238000004088 simulation Methods 0.000 abstract description 13
- 230000008859 change Effects 0.000 abstract description 12
- 230000003044 adaptive effect Effects 0.000 abstract description 7
- 230000035772 mutation Effects 0.000 abstract description 3
- 238000002474 experimental method Methods 0.000 abstract 1
- 238000003384 imaging method Methods 0.000 description 8
- 238000010586 diagram Methods 0.000 description 7
- 238000004519 manufacturing process Methods 0.000 description 7
- 230000006870 function Effects 0.000 description 6
- 230000003287 optical effect Effects 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 238000000342 Monte Carlo simulation Methods 0.000 description 3
- 238000012360 testing method Methods 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 241000221035 Santalaceae Species 0.000 description 1
- 235000008632 Santalum album Nutrition 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 238000000528 statistical test Methods 0.000 description 1
- 230000026676 system process Effects 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 230000001052 transient effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
- G01B11/00—Measuring arrangements characterised by the use of optical techniques
- G01B11/002—Measuring arrangements characterised by the use of optical techniques for measuring two or more coordinates
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Computer Hardware Design (AREA)
- Evolutionary Computation (AREA)
- Geometry (AREA)
- General Engineering & Computer Science (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to an airborne photoelectric system passive positioning filtering algorithm to the ground under a time-varying observation noise condition, and provides a combined self-adaptive extended Kalman filtering algorithm to realize passive positioning of a plurality of target points aiming at an unmanned airborne photoelectric system with time-varying observation noise. Aiming at the gradual change of observation noise, the invention balances the estimated quantity and the predicted quantity of an observation model according to the size of a dynamic residual error, thereby adjusting an observation covariance matrix in real time; aiming at the mutation of the observation noise, an adaptive forgetting factor is introduced into the observation covariance prediction to reduce the influence of the unmanned aerial vehicle working condition change on the geographic position estimation value of the photoelectric system. Finally, simulation experiment and actual measurement flight experiment results show that the algorithm has the characteristics of high precision, high instantaneity and strong robustness, can overcome the influence caused by nonlinear disturbance of a photoelectric system, and effectively improves the positioning precision of geographic positions of a plurality of target points.
Description
Technical Field
The invention relates to the field of positioning filtering algorithms, in particular to a passive positioning filtering algorithm of an airborne photoelectric system to ground under a time-varying observation noise condition.
Background
With the rapid development of airborne measurement systems, information exchange technology and computer vision, a new generation of unmanned aerial vehicle carrying a high-precision airborne photoelectric platform is moving towards modularization, light weight and intellectualization. The airborne photoelectric system is used as one of important means for situation awareness of unmanned aerial vehicles, and plays an increasingly critical role in important fields such as military reconnaissance, aerial remote sensing, resource detection and the like. The utilization of an onboard photoelectric system to accurately position a target point in an area becomes one of key technologies of the unmanned aerial vehicle.
At present, most traditional algorithms rely on distance to position a target point, and the distance between the unmanned aerial vehicle and the target point is acquired through a laser range finder, so that the three-dimensional geographic coordinates of the target point are calculated. Sandalwood and the like analyze and study the positioning error of the autonomous positioning technology of the target point of the airborne photoelectric measurement platform by means of the laser ranging value of the ground target point relative to the unmanned aerial vehicle and combining the pose and visual axis information of the unmanned aerial vehicle. Liu et al use distance information to solve the problem of non-linear estimation of passive positioning and tracking by a single observer. Zhang He and the like propose a direct-to-ground target point positioning algorithm based on laser ranging equipment, and high-precision target point positioning can be realized without accurate photoelectric platform posture information. Chen Chen and the like propose to establish a weighted error equation for continuous observation data under a limited observation condition, so that the accuracy of positioning a ground target point is effectively improved.
However, in practical use, the effective working distance of the laser range finder is limited, and good meteorological conditions are often required in the range finding process. In addition, with the improvement of the concealment performance and the self safety requirement of the unmanned aerial vehicle, the unmanned aerial vehicle is required not to be in optical and electrical communication with the outside in the task execution process, so that the risk of being discovered and intercepted is reduced. Therefore, under the condition that the distance information of the target points cannot be acquired, the high-precision passive real-time positioning is carried out on a plurality of target points by utilizing the visible light imaging system and the sensing information of the unmanned aerial vehicle, so that the method becomes a key problem to be solved in the development process of the current unmanned aerial vehicle perception technology.
The passive target point positioning method is widely and deeply researched by students at home and abroad so as to improve the target point positioning precision of the photoelectric platform under the passive condition. Sun Hui the target point positioning mathematical model from the image plane coordinates to the geodetic coordinates of the photoelectric platform imaging system is successfully constructed through linear transformation among different coordinate systems. Bai Guanbing and the like construct an ellipsoid model based on target points according to the digital elevation model, and finally realize real-time positioning of a plurality of target points in the same field of view by utilizing Kalman filtering. The algorithm adopts a Kalman Filter (KF) state estimation algorithm, and the KF algorithm can not deal with the nonlinear problem because the measurement equation of the system is very complex when the airborne photoelectric platform performs target point positioning. Therefore, mu Shaoshuo et al propose an extended kalman filter (extended Kalman filter, EKF) algorithm based on KF algorithm to estimate the geographic location of the ground target point. Liu Zhiming and the like accomplish pixel registration and continuous positioning within the region by multiple scan imaging of the target point region using a stereo imaging algorithm and an extended kalman filter. However, the EKF algorithm requires reasonable setting of the system noise matrix and measurement noise to suppress the system noise, is typically calibrated through extensive experimentation, and requires setting it to a fixed value. When the nonlinear disturbance exists in the photoelectric system due to the change of the running working condition of the airplane, the EKF cannot adaptively adjust the noise matrix containing time-varying characteristics, so that the positioning accuracy of the target point is reduced.
Aiming at the problem that when the unmanned aerial vehicle photoelectric system performs passive positioning of multiple target points, time-varying characteristics exist in system observation noise, the invention provides a combined self-adaptive extended Kalman filtering algorithm (Joint Adaptive Extended Kalman Filtering, JAEKF) for dynamic estimation of the unmanned aerial vehicle photoelectric system on the target point position, and mainly carries out the following research: 1) For the problem of observation noise in positioning, the invention researches a geographic position estimation strategy of a target point based on EKF, and reduces the influence of a visual axis measurement error on positioning precision; 2) For the gradual change of observation noise, the estimation amount and the prediction amount of an observation model are balanced according to the size of a dynamic residual covariance matrix, so that the observation covariance matrix is adjusted in real time; 3) Further, for the variability of the observation noise, an adaptive forgetting factor is introduced into the observation covariance prediction, so that the influence of the unmanned aerial vehicle working condition change on the geographic position estimated value of the photoelectric system is reduced.
Disclosure of Invention
Aiming at the problems and requirements, the invention provides a passive positioning filtering algorithm for the ground of an airborne photoelectric system under the condition of time-varying observation noise.
In order to solve the technical problems, the invention adopts the following technical scheme:
an airborne photoelectric system passive positioning filtering algorithm to the ground under the time-varying observation noise condition comprises the following steps:
determining initial position coordinates of the target point in a geodetic coordinate system, and carrying out real-time positioning iterative updating on the target point by means of self-adaptive extended Kalman filtering according to the position priori value of the target point at the last moment to obtainkUpdating position of time target point after filteringAnd covariance matrix->，/>The solution formula of (2) is:
；
in the method, in the process of the invention,is at the target pointkThe position observation quantity of the moment is obtained by an image tracker, < >>Is thatkA moment innovation vector; />Is composed ofk-1 observations of the target point position at time, +.>Is composed ofk-1 moment tokObservation of the target point position at the moment, +.>Is thatk-1 linearization matrix of nonlinear observation function at moment, < >>Is thatkThe priori value of the target point position at the moment-1 is obtained after repeated iterative updating according to the initial position of the target point; />Is thatk-1 moment tokPredicted value of target point position at moment; />Is composed ofk-1 moment tokA state transition matrix of the moment target point; />Is thatkA priori error covariance matrix of the position of the target point at the moment; />Is composed ofk-1 moment tokError in time-of-day target point positionA difference covariance matrix predictor; />Is thatk-1 moment tokTranspose of state transition matrix of moment target point; />Is thatkTime-of-day observed noise covariance;nto calculate the window length;kresidual vector at time ∈>；/>Is an intermediate variable +.>For the Kalman gain matrix at time k, < ->Is thatk-system noise at time 1W k-1 Is used for the co-variance matrix of (a),is a unit matrix;
λ k the self-adaptive forgetting factor is calculated by the following formula；
In the method, in the process of the invention,αis the maximum value of forgetting factors;γis the amplification factor;is thatkTime windowing innovation covariance, +.>The specific calculation formula of (2) is as follows:
。
further, the method comprises the steps of,h(·) As a nonlinear function of the observation of the optoelectronic platform,h(x) The specific formula of (2) is:
xfor the coordinates of the target point in the geodetic coordinate system, setx=[ L,B,H] T ，LAs the longitude information, there is provided,Bas the latitude information, it is possible to provide,His elevation information, the coordinates of the target point in the camera coordinate system arex c ,y c ,z c ] T Then:
；
（x c ,y c ,z c ) The calculation formula of (2) is as follows:
；
in equivalent focal length、/>Respectively camera focal length +.>Horizontal and vertical dimensions of the pixels>、/>Ratio of;for the transformation matrix of the body coordinate system into the camera coordinate system,/for the camera coordinate system>The transformation matrix from the navigation coordinate system to the machine body coordinate system; />Transformation matrix from geocentric earth fixed coordinate system to navigation coordinate system>A first eccentricity that is an ellipsoidal meridian ellipse of the earth; />Radius of curvature of circle of mortise and tenon, whereinIs an ellipsoidal long half shaft->Is an ellipsoidal short half shaft, and the units are meters.
After the technical scheme is adopted, compared with the prior art, the invention has the following advantages:
aiming at the time-varying characteristic of the observation noise of an unmanned aerial vehicle photoelectric system, the invention provides a combined self-adaptive extended Kalman filtering (JAEKF) algorithm for dynamic estimation of the photoelectric system on the geographic position of a target point. For the gradual change of observation noise, the JAEKF algorithm balances the estimated quantity and the predicted quantity of an observation model according to the size of a dynamic residual covariance matrix, so as to adjust the observation covariance matrix in real time; for the variability of observation noise, an adaptive forgetting factor is introduced into the observation covariance prediction, so that the influence of the unmanned aerial vehicle working condition change on the geographic position estimation value of the photoelectric system is reduced. Simulation and actual measurement test results show that the algorithm has the characteristics of high precision, high instantaneity and strong robustness, and can overcome the influence caused by nonlinear disturbance of a photoelectric system when the working condition is obviously changed due to rapid maneuver of the unmanned aerial vehicle, has better observation noise tracking capability and error convergence performance, and effectively improves the estimation precision of geographic positions of a plurality of target points. The invention will now be described in detail with reference to the drawings and examples.
Drawings
FIG. 1 is a schematic diagram of GCF and ECEF coordinate systems;
FIG. 2 is a schematic diagram of ECEF and NED coordinate systems;
FIG. 3 is a schematic diagram of the HRD, C and I coordinate systems;
FIG. 4 is a JAEKF algorithm flow;
FIG. 5 is a schematic illustration of a simulated flight trajectory and measurement target points;
FIG. 6 is a schematic view of simulated attitude angles;
FIG. 7 is a setpoint and its error frequency distribution map, wherein (a) is a setpoint distribution map; (b) drawing of longitude error frequency distribution map; (c) drawing is a latitude error frequency distribution diagram;
FIG. 8 is a schematic diagram of a round probability error simulation result;
fig. 9 is a diagram of the positioning root mean square error.
Detailed Description
The principles and features of the present invention are described below with reference to the drawings, the examples are illustrated for the purpose of illustrating the invention and are not to be construed as limiting the scope of the invention.
Aiming at the time-varying characteristic of the observation noise of an unmanned aerial vehicle photoelectric system, the invention provides a combined self-adaptive extended Kalman filtering (JAEKF) algorithm for dynamic estimation of the photoelectric system to the geographic position of a target point, and the main content of the method is as follows:
firstly, establishing an unmanned aerial vehicle photoelectric imaging model:
the positioning process of the target point mainly involves 6 basic inertial coordinate systems: a Geodetic (GCF) coordinate system, a geodetic (ECEF) coordinate system, a Navigation (NED) coordinate system, an airframe (HRD) coordinate system, a camera coordinate system (C), an image (I) coordinate system. The formula for transforming a coordinate point in the A coordinate system to the B coordinate system is as follows:
（1）；
wherein:is a transformation matrix from an A coordinate system to a B coordinate system, (-) is shown in the specificationx A ,y A ,z A ） T And%x B ,y B ,z B ） T The coordinates of the same point in different coordinate systems are respectively obtained.
As shown in fig. 1, at any point in the known spaceKIn the GCF coordinate system O gcf -LBHLocation information of (c) includes longitude informationLLatitude informationBElevation informationHGeodetic coordinates @L,B,H） T The earth coordinates of the earth centerx ecef ,y ecef ,z ecef ） T The coordinate transformation mathematical relationship of (a) is as follows
（2）；
Wherein: a first eccentricity that is an ellipsoidal meridian ellipse of the earth; is the curvature radius of the mortise unitary circle. Wherein the method comprises the steps ofa= 6378137 is the major half axis of an ellipsoid,b= 6356752.31 is an ellipsoidal minor half axis, all in meters.
The origin of the NED coordinate system is the position of the drone,X ned shaft and method for producing the sameY ned The axes point to north and east respectively,Z ned the axis is downward perpendicular to the earth's surface. As can be seen from FIG. 2, the earth-centered earth-fixed coordinatex ecef ,y ecef ,z ecef ） T To navigation coordinates [ ]x ned ,y ned , z ned ） T The transformation matrix of (a) is:
（3）；
NED coordinate systemO ned - X ned Y ned Z ned Origin of (1) and HRD coordinate systemO hrd - X hrd Y hrd Z hrd Is coincident with the origin of (a),X hrd shaft and method for producing the sameY hrd The shaft points to the direction of the nose and the right wing of the unmanned aerial vehicle respectively,Z hrd the axis is vertical to the plane of the unmanned plane and downward. As can be seen from fig. 3, the navigation coordinates #x ecef ,y ecef ,z ecef ） T To the machine body coordinates
（x ned ,y ned ,z ned ） T The transformation matrix of (a) is:
（4）；
c coordinate systemO c - X c Y c Z c Is the optical center of the photoelectric platformO c ，X c Shaft and method for producing the sameY c Axis and image coordinate system described laterxShaft and method for producing the sameyThe axes are parallel and the axes are parallel,Z c the axis coincides with the optical axis of the optoelectronic platform. Because the photoelectric imaging platform is arranged in the two-axis frame, only the high angle and the low angle of the inner frame of the camera in the platform need to be consideredβAzimuth angle of outer frameαThe machine body coordinatesx hrd ,y hrd ,z hrd ） T To the camera coordinatesx c ,y c ,z c ) T Is a transform matrix of (a)The method comprises the following steps:
（5）；
wherein: t= [t x ,t y ,t z ]A position vector from the origin of the HRD coordinate system to the origin of the C coordinate system is called a translation vector.
I coordinate systemI-xyIn the upper left corner of the imageThe vertex is the origin point,xshaft and method for producing the sameyThe axes coincide with the long and short sides of the image, respectively. To facilitate the alignment of image pointspAnd corresponding object pointPPerforming mutual conversion of spatial positions, and introducing an image physical coordinate system into the coordinate transformationo-x w y w . The origin of the physical coordinate system of the image is the intersection point of the optical axis and the image planeo，x w Shaft and method for producing the samey w The shafts are respectively connected withxShaft and method for producing the sameyThe axes are parallel. As can be seen from FIG. 3, a certain point in spacePThe coordinates in the C coordinate system are%x c ,y c ,z c ) And coordinates in the physical coordinate system of the imagex w ，y w ) The relationship between them is as follows:
（6）；
wherein:fis the focal length of the camera.
Then the image pointpThe image coordinates of (a) are expressed as [ (]u,v) Physical coordinates of corresponding imagex w ,y w ) The relation of (2) is:
（7）；
thus, the image pointpPixel coordinates of [ ]u,v) And object pointpCamera coordinates of [ (]x c ,y c ,z c ) The relation between the two is:
（8）；
wherein: equivalent focal lengthf x ，f y Respectively is focal lengthfTransverse and longitudinal dimensions of the picture elementsd x ，d y Ratio of the two components.
Converting formula (8) into a matrix form, yields:
（9）；
assuming that the unmanned aerial vehicle photoelectric system continuously measures a plurality of target points in the field of view for a plurality of times, the navigation coordinates of the target points are selected as state quantities of the system because the estimated object is the geographic position information of the target points, namely
x k =[ L k ,B k ,H k ] T （10）；
The state equation of the system is:
（11）；
wherein: subscript ofkAndk-1 respectively representkTime of day and time of dayk-moment 1;is the slavek-1 moment tokA state transition matrix at a moment; />Is a noise driving matrix; />Is system process noise. For stationary target points, it is apparent that:
（12）；
wherein:Q k-1 is the system noiseW k-1 Is a covariance matrix of (a).
The observed quantity of the system, namely the pixel coordinates of the target point under the image coordinate system, can be obtained by an image tracker, and the mathematical expression is as follows:
（13）；
the observation equation is:
（14）；
wherein:V k-1 for measuring noise;h(·) Is a nonlinear function of the observation of the photoelectric platform. In addition, in the case of the optical fiber,V k-1 is the covariance matrix of (2)R k-1 。
The following is the followingh(x k ) Is a solution process of (1):
suppose the target pointPCoordinates in the GCF coordinate systemx k =[ L k ,B k ,H k ] T Coordinates in C coordinate system [x c , y c ,z c ] T As an intermediate variable, from formula (9):
（15）；
next, the parameters in the formula (15)x c ,y c ,z c ) Can be calculated by the formula (16):
（16）；
the EKF algorithm obtains a linear approximation solution by taylor expansion of a nonlinear function, which can be used to update the state estimate of the filter at each time step using a linearized version of the state and observation equations. However, the EKF algorithm needs to reasonably set a system noise matrix and measure noise to suppress the noise of the system, and when the operation condition of the aircraft changes, the algorithm is difficult to adaptively adjust the noise matrix, so that the positioning accuracy of the target point is reduced. Therefore, the invention provides a joint self-adaptive extended Kalman filtering algorithm, which realizes self-adaptive updating of observation noise and ensures steady-state performance of the algorithm through the joint self-adaptive algorithm of the observation noise matrix and the forgetting factor, thereby improving the positioning precision of a target point and the error convergence speed when noise is suddenly changed.
According to the forward Euler method, discrete forms of the system state equation and the measurement equation are obtained:
（17）；
wherein:klinearization matrix of nonlinear observation function at-1 moment。
Equation (17) evolves the problem of target point positioning of the drone into a problem of optimal estimation of target point position by multiple measurements. The state equation of the system is linear, but the measurement equation is nonlinear, so that the position of the target point is recursively estimated by adopting an EKF, and the calculation process comprises the following two steps:
(1) Prediction
Predicting the state and covariance by using the positioning result of the previous period:
（18）；
(2) Updating
Correcting the predicted state and covariance by using the innovation vector:
（19）；
in the method, in the process of the invention,is at the target pointkThe position observation quantity of the moment is obtained by an image tracker, < >>Is thatkA moment innovation vector; />Is composed ofk-1 moment tokObservation of the target point position at the moment, +.>Is thatk-1 linearization matrix of nonlinear observation function at moment, < >>Is thatkThe priori value of the target point position at the moment-1 is obtained after repeated iterative updating according to the initial position of the target point; />Is thatk-1 moment tokPredicted value of target point position at moment; />Is composed ofk-1 moment tokA state transition matrix of the moment target point; />Is thatkA priori error covariance matrix of the position of the target point at the moment;is composed ofk-1 moment tokAn error covariance matrix predicted value of the target point position at the moment; />Is thatk-1 moment tokTranspose of state transition matrix of moment target point; />Is thatkTime-of-day observed noise covariance; />Is an intermediate variable +.>For the Kalman gain matrix at time k, < ->Is an identity matrix.
Aiming at the gradual change characteristic of the observation noise of the photoelectric system, on the basis of an EKF algorithm, an observation matrix is adjusted in real time according to the size of a dynamic residual between the observed quantity and the predicted quantity of the system, namely, the covariance of the observation noise is estimated through the weighting processing of an observation residual sequence at each discrete moment in a window. The calculation formula is as follows:
（20）；
in the method, in the process of the invention,nto calculate the window length;kresidual vector at time instant。
Aiming at the mutation characteristic of the observation noise of the photoelectric system, the self-adaptive forgetting factor is introduced in the observation covariance prediction, the transient response speed of the algorithm to the mutation noise is improved, the defect that the stability of the algorithm is reduced by the invariable forgetting factor is overcome, and therefore the influence of the working condition change of the unmanned plane on the geographic position estimated value of the photoelectric system is reduced. The calculation formula of the self-adaptive forgetting factor is as follows:
（21）；
in the method, in the process of the invention,αis the maximum value of forgetting factors;γis the amplification factor;is thatkThe moment windowing innovation covariance is calculated by the following specific formula:
（22）；
the error covariance matrix is
（23）；
From equation (22), the adaptive forgetting factor is about 1 when the system enters steady state, so the proposed algorithm has good stability while rapidly processing abrupt noise. By the formula (19)And (4) replacing the algorithm with the formula (23) and combining the formula (19) and the formula (20), namely the constructed combined self-adaptive extended Kalman filtering algorithm, wherein the specific flow chart is shown in figure 4.
The specific data of the simulation experiment of the invention are as follows:
in order to verify the accuracy and the effectiveness of the passive target point positioning algorithm of the unmanned aerial vehicle photoelectric platform, a Monte Carlo statistical test method is adopted for simulation test. Assuming that the geographical location of the target point is (31.000000 °n,114.000000 °e,50.00 m), the drone flies clockwise around the target point at a altitude of 1000m and with a hover radius of 577m, roll angle is 45 °, pitch angle is 0 °, its flight trajectory and measurement target point are shown in fig. 5. The imaging times of the unmanned aerial vehicle photoelectric platform are set to be 200 times, the depression angle of the optical axis is 60 degrees, and the imaging distance is about 1200m.
The unmanned aerial vehicle is provided with a GPS sensor and an inertial navigation sensor which can measure the accurate geographic position and the accurate attitude of the unmanned aerial vehicle, and the measurement error simulation parameters are shown in table 1. According to the error simulation parameters and the flight path of the unmanned aerial vehicle, the attitude angles of the unmanned aerial vehicle and the video camera and the error probability distribution condition are simulated and analyzed by a Monte Carlo method, and the change rule of simulation data is shown in figure 6.
Table 1 measurement error simulation parameters
The focal length of the optoelectronic platform during simulation was 50mm, the pixel size was 2.9 μm, the parameter settings of Table 1 were referenced, and takenn=100, the target point at the center of the image plane was located, and the monte carlo simulation analysis result is shown in fig. 7. The blue dispersion points in FIG. 7 (a) represent all GPS coordinate points of the Monte Carlo simulationA total of 20000; fig. 7 (b) and (c) are error frequency distribution diagrams of a longitude calculated value and a true value, and a latitude calculated value and a true value, respectively, and a red curve is a frequency fitting curve in the form of a normal distribution.
Supposing%L i ,B i ) The result is the ith GPS positioning resultL t ,B t ) The location standard deviation of longitude and latitude is respectively:
from the data in FIG. 7, a longitude positioning standard deviation of 2.5601 ×10 was calculated -4 The standard deviation of latitude positioning is 2.1735 multiplied by 10 -4 And (3) degree. The earth ellipsoid model defined by WGS-84 shows that the round probability error capable of reflecting the positioning accuracy of the geographic position of the target point is
In the method, in the process of the invention,and->The curvature radius of the mortise and tenon circle and the meridian circle respectively,H t for the height of the target point,B t is the latitude of the target point.
Assuming that the error of the height of the target point area is 25m, the covariance matrix of the algorithm is calculatedP 1 Set as [ diag (0.0001,0.00014,25)]The observation process noise was set to [ diag (1, 5)]. In order to verify the improvement of the positioning accuracy of the target point by the proposed algorithm, the EKF, UKF and JAEKF are respectively used for comparison analysis, and the obtained round probability error simulation result is shown in FIG. 8.
As can be seen from fig. 8, the EKF cannot effectively and accurately recognize and process the observation noise when solving the problem of positioning the target point of the photoelectric platform, and the simulation result of the round probability error is maximum and the fluctuation is severe. UKF has better performance to nonlinear and non-Gaussian problems, reduces errors, but cannot effectively track observation noise when the working condition of an airplane changes drastically, and has poor stability of algorithm results and larger fluctuation. AEKF dynamically adjusts observed noise statistics by introducing adaptive control, with further reduction in errors and fluctuations. The JAEKF introduces a self-adaptive forgetting factor in the observation covariance prediction, the error and fluctuation are minimum, and the error is more converged in the later filtering period, which shows that the algorithm of the invention further improves the stability of the observation noise estimation.
The root mean square error is an index reflecting the positioning performance of the algorithm, and the calculation formula is as follows:
in the method, in the process of the invention,mfor the number of information sequences,for the estimation of the positioning information, +.>Is the true value of the positioning information. 100 Monte Carlo simulation calculations were performed for each of the above algorithms and the root mean square errors for the different algorithms were compared (Root Mean Square Error, RMSE) as shown in FIG. 9.
In fig. 9, the left vertical axis represents the root mean square error of longitude and latitude, the right vertical axis represents the root mean square error of round probability, the solid line frame corresponds to the EKF method, the thick dotted line frame corresponds to the UKF method, the dash-dot line frame corresponds to the AEKF method, the thin dotted line frame corresponds to the JAEKF method of the present invention, and in the figure, it can be seen that the root mean square error of the algorithm provided by the present invention is minimum, and has stronger robustness. For positioning accuracy, JAEKF was increased by 43.9%, 30.3% and 14.8% compared to EKF, UKF and AEKF, respectively, on average.
Aiming at the time-varying characteristic of the observation noise of an unmanned aerial vehicle photoelectric system, the invention provides a combined self-adaptive extended Kalman filtering (JAEKF) algorithm for dynamic estimation of the photoelectric system on the geographic position of a target point. For the gradual change of observation noise, the JAEKF algorithm balances the estimated quantity and the predicted quantity of an observation model according to the size of a dynamic residual covariance matrix, so as to adjust the observation covariance matrix in real time; for the variability of observation noise, an adaptive forgetting factor is introduced into the observation covariance prediction, so that the influence of the unmanned aerial vehicle working condition change on the geographic position estimation value of the photoelectric system is reduced. Simulation and actual measurement test results show that the algorithm has the characteristics of high precision, high instantaneity and strong robustness, and can overcome the influence caused by nonlinear disturbance of a photoelectric system when the working condition is obviously changed due to rapid maneuver of the unmanned aerial vehicle, has better observation noise tracking capability and error convergence performance, and effectively improves the estimation precision of geographic positions of a plurality of target points.
The foregoing is illustrative of the best mode of carrying out the invention, and is not presented in any detail as is known to those of ordinary skill in the art. The protection scope of the invention is defined by the claims, and any equivalent transformation based on the technical teaching of the invention is also within the protection scope of the invention.
Claims (2)
1. The passive positioning filtering algorithm for the ground of the airborne photoelectric system under the time-varying observation noise condition is characterized by comprising the following steps of:
determining initial position coordinates of the target point in a geodetic coordinate system, and carrying out real-time positioning iterative updating on the target point by means of self-adaptive extended Kalman filtering according to the position priori value of the target point at the last moment to obtainkUpdating position of time target point after filteringAnd covariance matrix->，/>The solution formula of (2) is:
；
in the method, in the process of the invention,is at the target pointkThe position observation quantity of the moment is obtained by an image tracker, < >>Is thatkA moment innovation vector; />Is composed ofk-1 observations of the target point position at time, +.>Is composed ofk-1 moment tokObservation of the target point position at the moment, +.>Is thatk-1 linearization matrix of nonlinear observation function at moment, < >>Is thatkThe priori value of the target point position at the moment-1 is obtained after repeated iterative updating according to the initial position of the target point; />Is thatk-1 moment tokPredicted value of target point position at moment; />Is composed ofk-1 moment tokA state transition matrix of the moment target point; />Is thatkA priori error covariance matrix of the position of the target point at the moment; />Is composed ofk-1 moment tokAn error covariance matrix predicted value of the target point position at the moment; />Is thatk-1 moment tokTranspose of state transition matrix of moment target point; />Is thatkTime-of-day observed noise covariance;nto calculate the window length;kresidual vector at time ∈>；/>Is an intermediate variable +.>For the Kalman gain matrix at time k, < ->Is thatk-system noise at time 1W k-1 Covariance matrix of>Is a unit matrix;
λ k the self-adaptive forgetting factor is calculated by the following formula；
In the method, in the process of the invention,αis the maximum value of forgetting factors;γis the amplification factor;is thatkTime windowing innovation covariance, +.>The specific calculation formula of (2) is as follows:
。
2. the method of claim 1, wherein the on-board electro-optical system passive positioning filtering algorithm is based on the time-varying observation noise condition,h(·) As a nonlinear function of the observation of the optoelectronic platform,h(x) The specific formula of (2) is:
xfor the coordinates of the target point in the geodetic coordinate system, setx=[ L,B,H] T ，LAs the longitude information, there is provided,Bas the latitude information, it is possible to provide,His elevation information, the coordinates of the target point in the camera coordinate system arex c ,y c ,z c ] T Then:
；
（x c ,y c ,z c ) The calculation formula of (2) is as follows:
；
in equivalent focal length、/>Respectively camera focal length +.>Horizontal and vertical dimensions of the pixels>、/>Ratio of; />For the transformation matrix of the body coordinate system into the camera coordinate system,/for the camera coordinate system>The transformation matrix from the navigation coordinate system to the machine body coordinate system;transformation matrix from geocentric earth fixed coordinate system to navigation coordinate system>A first eccentricity that is an ellipsoidal meridian ellipse of the earth; />Radius of curvature of circle of mortise and tenon, whereinIs an ellipsoidal long half shaft->Is an ellipsoidal short half shaft, and the units are meters.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311780923.9A CN117455960B (en) | 2023-12-22 | 2023-12-22 | Passive positioning filtering method for airborne photoelectric system to ground under time-varying observation noise condition |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311780923.9A CN117455960B (en) | 2023-12-22 | 2023-12-22 | Passive positioning filtering method for airborne photoelectric system to ground under time-varying observation noise condition |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117455960A true CN117455960A (en) | 2024-01-26 |
| CN117455960B CN117455960B (en) | 2024-03-08 |
Family
ID=89585914
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311780923.9A Active CN117455960B (en) | 2023-12-22 | 2023-12-22 | Passive positioning filtering method for airborne photoelectric system to ground under time-varying observation noise condition |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117455960B (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117787021A (en) * | 2024-02-28 | 2024-03-29 | 中国人民解放军海军工程大学 | Laser far field energy density estimation method |
| CN117990112A (en) * | 2024-04-03 | 2024-05-07 | 中国人民解放军海军工程大学 | Unmanned aerial vehicle photoelectric platform target positioning method based on robust unscented Kalman filtering |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105491661A (en) * | 2015-12-10 | 2016-04-13 | 上海电机学院 | Improved Kalman filtering algorithm-based indoor positioning system and method |
| US20190014422A1 (en) * | 2016-07-06 | 2019-01-10 | Oticon A/S | Direction of arrival estimation in miniature devices using a sound sensor array |
| CN109275121A (en) * | 2018-08-20 | 2019-01-25 | 浙江工业大学 | A kind of Vehicle tracing method based on adaptive extended kalman filtering |
| CN109829938A (en) * | 2019-01-28 | 2019-05-31 | 杭州电子科技大学 | A kind of self-adapted tolerance volume kalman filter method applied in target following |
| CN110706265A (en) * | 2019-11-05 | 2020-01-17 | 中国人民解放军国防科技大学 | Maneuvering target tracking method for improving SRCKF strong tracking filtering |
| US20210199752A1 (en) * | 2018-04-05 | 2021-07-01 | Raytheon Company | System and method for intercepting an exo-atmospheric target using passive ranging estimation |
-
2023
- 2023-12-22 CN CN202311780923.9A patent/CN117455960B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105491661A (en) * | 2015-12-10 | 2016-04-13 | 上海电机学院 | Improved Kalman filtering algorithm-based indoor positioning system and method |
| US20190014422A1 (en) * | 2016-07-06 | 2019-01-10 | Oticon A/S | Direction of arrival estimation in miniature devices using a sound sensor array |
| US20210199752A1 (en) * | 2018-04-05 | 2021-07-01 | Raytheon Company | System and method for intercepting an exo-atmospheric target using passive ranging estimation |
| CN109275121A (en) * | 2018-08-20 | 2019-01-25 | 浙江工业大学 | A kind of Vehicle tracing method based on adaptive extended kalman filtering |
| CN109829938A (en) * | 2019-01-28 | 2019-05-31 | 杭州电子科技大学 | A kind of self-adapted tolerance volume kalman filter method applied in target following |
| CN110706265A (en) * | 2019-11-05 | 2020-01-17 | 中国人民解放军国防科技大学 | Maneuvering target tracking method for improving SRCKF strong tracking filtering |
Non-Patent Citations (2)
| Title |
|---|
| LAI-TIAN CAO.ET.: "Passive Positioning and Filtering Technology Using Relative Height Measurement Information", 《ARTIFICIAL INTELLIGENCE IN CHINA》, vol. 854, 22 March 2022 (2022-03-22), pages 678 * |
| 刘林峰等: "一种提高可见光遥感图像黑极性舰船目标检出率的方法", 《光学与光电技术》, vol. 21, no. 4, 26 May 2023 (2023-05-26), pages 59 - 66 * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117787021A (en) * | 2024-02-28 | 2024-03-29 | 中国人民解放军海军工程大学 | Laser far field energy density estimation method |
| CN117787021B (en) * | 2024-02-28 | 2024-05-07 | 中国人民解放军海军工程大学 | Laser far field energy density estimation method |
| CN117990112A (en) * | 2024-04-03 | 2024-05-07 | 中国人民解放军海军工程大学 | Unmanned aerial vehicle photoelectric platform target positioning method based on robust unscented Kalman filtering |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117455960B (en) | 2024-03-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN117455960B (en) | Passive positioning filtering method for airborne photoelectric system to ground under time-varying observation noise condition | |
| CN106871927B (en) | Unmanned aerial vehicle photoelectric pod installation error calibration method | |
| CN110470304B (en) | High-precision target positioning and speed measuring method based on unmanned aerial vehicle photoelectric platform | |
| CN112197761B (en) | High-precision multi-gyroplane co-location method and system | |
| AU2012328156B2 (en) | Identification and analysis of aircraft landing sites | |
| CN107132542B (en) | A kind of small feature loss soft landing autonomic air navigation aid based on optics and Doppler radar | |
| CN111123341B (en) | Three-dimensional co-location method for unmanned aerial vehicle group | |
| CN105698762A (en) | Rapid target positioning method based on observation points at different time on single airplane flight path | |
| CN111366148B (en) | Target positioning method suitable for multiple observations of airborne photoelectric observing and sighting system | |
| CN107727101B (en) | Three-dimensional attitude information rapid resolving method based on dual-polarized light vector | |
| CN110081875B (en) | Unmanned aerial vehicle autonomous navigation system and method imitating pigeon intelligence | |
| Xu et al. | Target location of unmanned aerial vehicles based on the electro-optical stabilization and tracking platform | |
| CN105180963A (en) | Unmanned plane remote measurement parameter correction method based on on-line calibration | |
| CN108896957A (en) | The positioning system and method in a kind of unmanned plane control signal source | |
| CN115291536B (en) | Visual unmanned aerial vehicle tracking ground target semi-physical simulation platform verification method | |
| Ouyang et al. | Cooperative navigation of UAVs in GNSS-denied area with colored RSSI measurements | |
| Hill et al. | Ground-to-air flow visualization using Solar Calcium-K line Background-Oriented Schlieren | |
| RU2513900C1 (en) | Method and device to determine object coordinates | |
| CN112268558B (en) | Active target positioning and speed measuring method based on unmanned aerial vehicle autonomous photoelectric platform | |
| CN115388890A (en) | Visual sense-based multi-unmanned aerial vehicle cooperative ground target positioning method | |
| CN112729305B (en) | Multi-target positioning method based on single aircraft seeker image information | |
| Sanna et al. | A novel ego-motion compensation strategy for automatic target tracking in FLIR video sequences taken from UAVs | |
| CN113514052A (en) | Multi-machine cooperation high-precision active target positioning method and system | |
| CN113503891A (en) | SINSDVL alignment correction method, system, medium and equipment | |
| CN110825117A (en) | High-precision unmanned aerial vehicle system and intelligent control method |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |