A binocular positioning method with unconstrained arrangement of light sources

Abstract

The present invention provides a binocular positioning method with unconstrained arrangement of light sources, which belongs to the field of computer vision positioning. It is a visible light positioning method that uses an unscented Kalman filter to fuse data from an inertial sensor and data from a binocular camera. The present invention first establishes a process equation in the framework of an unscented Kalman filter algorithm to predict the position and attitude angle of a target object in three-dimensional space, and then establishes a measurement equation based on the pixel image of the binocular camera to correct the position and heading angle of the visible light feature points of the target object calculated in the process equation. The position and attitude angle of the visible light feature points of the target object calculated by the present invention can maintain high accuracy even when the data accuracy of the six-axis inertial sensor is low and the position recognition noise of the visible light feature points in the pixel image is large; it has wide applicability and strong stability.

Description

Technical Field

[0001] The present invention belongs to the field of computer vision positioning and is a technology that uses images captured by binocular cameras to process scene depth information. This technology can determine the position and posture of the target object through the feature points of visible light in the digital images of the two cameras and the data of inertial sensors, thereby realizing the positioning or navigation of the object. Background Art

[0002] Inertial sensors and binocular cameras are two commonly used sensors for positioning and navigation. They use inertial force and optical imaging to obtain motion and spatial information, respectively. However, both inertial sensors and binocular cameras have their own drawbacks. For example, inertial sensors are affected by noise interference and drift, while binocular cameras can produce pixel errors at feature points due to calibration accuracy and lighting. Therefore, in positioning algorithms, data from these two sensors is often fused to improve algorithm stability and accuracy. However, this requires high-precision raw inertial sensor data or the three attitude angles of the target object, placing high demands on the inertial sensor data accuracy and even requiring the addition of a geomagnetic sensor, which significantly limits the deployable environment.

[0003] On this basis, the present invention proposes a visible light positioning algorithm that is suitable for combining low-precision inertial sensors and binocular cameras. It has strong stability and can achieve accurate position and posture calculation even in the presence of feature point image recognition noise. Summary of the Invention

[0004] The present invention aims to address the problem of low positioning accuracy and large attitude angle errors caused by low inertial sensor precision or noise in feature point image recognition in the fusion positioning algorithm of a binocular camera and an inertial sensor. The present invention targets a target object with three unconstrained visible light feature points (the relative positions of the three visible light feature points are unknown). By capturing the positions of the three visible light feature points of the moving target object in the camera pixel image using a binocular camera and the pitch and roll angles obtained by a six-axis inertial sensor, the present invention calculates the position and heading angle of the three visible light feature points of the target object in three dimensions.

[0005] In order to achieve the above object, the technical solution adopted by the present invention is:

[0006] A binocular positioning method with unconstrained arrangement of light sources is a visible light positioning method that uses an unscented Kalman filter to fuse data from an inertial sensor and data from a binocular camera. The present invention first establishes a process equation in the framework of an unscented Kalman filter algorithm to predict the position and attitude angle of a target object in three-dimensional space, and then establishes a measurement equation based on the pixel image of the binocular camera to correct the position and heading angle of the visible light feature points of the target object calculated in the process equation. The position and attitude angle of the visible light feature points of the target object calculated by the present invention can maintain high accuracy even when the data accuracy of the six-axis inertial sensor is low and the position recognition noise of the visible light feature points in the pixel image is large. Specifically, the following steps are included:

[0007] 1. Initialization

[0008] 1.1 Camera Calibration

[0009] Let the left and right cameras of the binocular camera be camera 1 and camera 2 respectively; perform binocular camera calibration on camera 1 and camera 2, and obtain the intrinsic parameters (focal length: f, pixel image center point: (u0, v0), pixel size: dx, dy) and extrinsic parameters (rotation matrix of camera 2 relative to camera 1: R) of the two cameras respectively. CCS (CCS: Camera Coordinate System, camera coordinate system), translation matrix: T CCS ), where the intrinsic parameters of camera 1 and camera 2 are the same, these parameters will be used in the following camera measurement equations;

[0010] 1.2 Establish the state vector of the unscented Kalman filter

[0011] A right-handed three-dimensional coordinate system is established with the optical center of the lens of camera 1 as the origin and the lens outward as the positive z-axis. This coordinate system is camera coordinate system 1 (CCS1). Similarly, camera coordinate system 2 (CCS2) is established on camera 2, and CCS1 is made the principal coordinate system. Let the state vector (hereinafter referred to as "state") of the kth period (hereinafter referred to as "time k") of the calculation cycle be χ k =[P 1,k P 2,k P 3,k T k β k ] T Among them, P i,k (i=1,2,3) is the coordinate of the i-th visible light feature point under CCS1 when it moves with the target object at the k-th moment; T k Denote the translation vector at time k; β kThe heading angle at time k is the first step in using the unscented Kalman filter. When k = 0, it is the initial state of the unscented Kalman filter, which can be set according to the distance and relative posture between the current binocular camera and the target object.

[0012] 1.3 Establishing the process equation of the unscented Kalman filter

[0013] First, the state vector χ at time k k (hereinafter referred to as state) establish the following equation:

[0014]

[0015] The left side of the equation is the state at time k; the right side of the equation is the equation for calculating the state at time k using the state at time k-1, P i,k-1 is the coordinate of the i-th visible light feature point in the three-dimensional space moving with the target object under CCS1 at time k-1; T k-1 Denote the translation vector at time k-1; R k Denote the rotation matrix at time k, through the pitch angle α at time k k and the roll angle γ k And the pitch angle at time k-1 is calculated using the following formula:

[0016]

[0017] , which is the Euler-Rodrigues formula; using Calculate the average value of the translation vector output of the first m cycles at time k to calculate T k Similarly, use To calculate the average value of the heading angle output of the first m cycles at time k, to calculate β k .w k is the noise of the predicted state at time k, which is automatically estimated by the unscented Kalman filter algorithm.

[0018] The above equations are collectively called process equations, written as χ k =F(χ k-1 ,α k ,γ k ). Establishing the process equation is the second step of using the unscented Kalman filter. This equation describes how to use the k-1 moment state χ k-1 Predict the state χ at time k k ;

[0019] 1.4 Establishing the measurement equation of the unscented Kalman filter

[0020] The camera images any point in the three-dimensional space onto the camera's pixel image. If the positional relationship between the coordinates of any point in the three-dimensional space and the camera's pixel image can be obtained, then after the coordinates of the point on the camera's pixel image are known, partial information about the coordinates of the point in the three-dimensional space can be obtained. This information can be used with the Kalman filter algorithm to calculate the state χ predicted in 1.2. k Correction is performed, a process called measurement. (The relationship between the coordinates of a point in 3D space and the coordinates of the camera pixel image is one-way. Given the coordinates in 3D space, the coordinates of the camera pixel image can be calculated, but the reverse is not possible because a point in the camera pixel image corresponds to countless points on a straight line in 3D space.)

[0021] The binocular camera has been calibrated in 1.1, so:

[0022]

[0023] This matrix A is called the internal parameter matrix.

[0024] For the kth moment, the coordinates of the i-th feature point under CCS1 are: P i,k (i=1,2,3), can be changed to augmented form: P i,k =(x i,k ,y i,k ,z i,k ,1) T (i=1,2,3), the pixel coordinates of the point in the camera 1 image (superscript p1 is the pixel image of camera 1), is the horizontal and vertical coordinates of the point in the pixel image; P i,k (i=1,2,3) and The relationship is:

[0025]

[0026] Here we write the equation as: Where E3 is the third-order unit matrix. At the same time, we also need to obtain P i,k (i=1,2,3) coordinates on the pixel image of camera 2 (superscript p2 is the pixel image of camera 2), first calculate P i,k Coordinates of (i=1,2,3) under CCS2 The coordinates under are (the superscript CCS2 is the coordinates under camera coordinate system 2):

[0027]

[0028] but for:

[0029]

[0030] The formula is recorded as:

[0031] Finally, the equation and Merge into:

[0032]

[0033] This equation is the measurement equation. The measurement equation converts the point coordinates (P i,k (i=1,2,3)) is converted into the pixel image coordinates of camera 1 and camera 2 of the binocular camera, that is, the predicted state χ in 1.2 can be k P in i,k (i=1,2,3) calculate the pixel image coordinates of camera 1 and camera 2. Get from the binocular camera And obtain R from the binocular camera calibration results CCS 、T CCS After that, the unscented Kalman filter can be used to filter P i,k (i=1,2,3) for correction.

[0034] 1.5 Initialization parameters of the unscented Kalman filter

[0035] In 1.2, 1.3, and 1.4, the Kalman filters of the state, process equations, and measurement equations are established. It is also necessary to determine the initial values ​​of the unscented Kalman filter, which are the starting position and attitude of the target object. It is also necessary to set the process noise covariance matrix S based on the accuracy of the process equation in describing the changes in the coordinates and attitude angles of the three feature points during the actual movement of the target object. Correspondingly, the measurement noise covariance matrix L is set based on the accuracy of the measurement equation in describing the camera imaging.

[0036] 2. Unscented Kalman Filter Iteration

[0037] 2.1 Obtaining pitch and roll angles

[0038] For a target object with three unconstrained visible light feature points, a six-axis inertial sensor is installed to obtain three real-time changing attitude angles during the three-dimensional space movement, namely: pitch angle, heading angle, and roll angle. The three attitude angles at any k-th moment are recorded as: α k , β k , γ k See the accompanying figure. Figure 2Among them, for the six-axis inertial sensor, the pitch angle and roll angle of the target object can be accurately obtained, but the heading angle generally has a large cumulative error. Therefore, the present invention only uses the obtained pitch angle and roll angle, and simultaneously calculates the heading angle without cumulative error;

[0039] 2.2 Image Acquisition

[0040] For a target object with three unconstrained visible light feature points, a binocular camera is used to capture the positions of the three visible light feature points in the camera pixel image during its movement. The coordinates of the i-th visible light feature point at any k-th time in the pixel image of the j-th camera are marked as P i pj (i=1,2,3; j=1,2) The coordinates of the image (the superscript pj is the pixel image of the jth camera).

[0041] 2.3 Calculate the sigma point set of the state at time k-1

[0042] The optimal state X at time k-1 k-1 is dispersed into 2n+1 state sigma point sets according to the following rules:

[0043]

[0044] χ l,k-1 (l=0,1,...,2n) is the optimal state X at time k-1 k-1 The sigma point set of the state at time k-1; n is X k-1 The number of members; λ is the diagonal factor, which is a constant and is calculated as λ=ε 2 (n+κ)-n, ε is the distribution range factor, which is a constant, κ is the distribution factor, which is a constant; Q k-1 is the optimal state covariance matrix at time k-1, which is automatically estimated by the unscented Kalman filter.

[0045] 2.4 Calculating the sigma point set of the k-time process

[0046] In 1.3, the process equation F(χ k-1 ,α k ,γ k ,w k-1 ), the sigma point set χ of the k-1 moment state in 2.3 l,k-1 (l=0,1,...,2n) is brought into the equation to calculate the update of the sigma point set of the state in the process equation, which is the sigma point set χ of the process at time k l,k (l=0,1,...,2n):

[0047]

[0048] Where W l m (l=0,1,...,2n) is the mean weight vector, and the calculation formula is:

[0049]

[0050] is the sigma point set χ of the process l,k (l=0,1,...,2n) in the mean weight vector W l m The mean of the sigma point set of the process under (l=0,1,...,2n).

[0051] 2.5 Calculate the state covariance matrix at time k

[0052]

[0053] is the state covariance matrix at time k; W l c (l=0,1,...,2n) is the variance weight vector, and the calculation formula is:

[0054]

[0055] S is the process noise covariance matrix, set in 1.5.

[0056] 2.6 Calculate the sigma point set of the measurement at time k

[0057] Similarly, the measurement equation H(A,P) is established in 1.4. i,k ,R CCS ,T CCS ), the sigma point set χ of the k-time process in 2.4 l,k (l=0,1,...,2n) is brought into the equation to calculate the update of the point set in the measurement equation (the sigma point set measured at time k) Z l,k (l=0,1,...,2n):

[0058]

[0059] Z l,k (l=0,1,...,2n) is the process sigma point set at time k, is the mean weight vector W at time k l m The mean of the process sigma point set under (l=0,1,...,2n).

[0060] 2.7 Calculate the measurement covariance matrix at time k

[0061]

[0062] is the measurement covariance matrix at time k; L is the measurement noise covariance matrix, which is set in 1.5.

[0063] 2.8 Calculate the Kalman gain at time k

[0064]

[0065] is the state-measurement joint covariance matrix at time k; K k is the Kalman gain at time k.

[0066] 2.9 Calculate the optimal state at time k and repeat iterations

[0067] 2.9.1 Calculating the optimal state at time k

[0068]

[0069]

[0070] X k is the optimal state at time k; z k is the coordinate of the pixel image of the visible light feature point at time k obtained from 2.2 P k is the covariance matrix of the optimal state at time k.

[0071] 2.9.2 Output and Iteration

[0072] After calculating the optimal state at time k and the optimal state covariance at time k in 2.9.1, the iteration at time k is completed, X k These are the coordinates and attitude angles of the three visible light feature points of the target object under CCS1. Let this k moment be k-1, the next moment be k, and return to 2.1 to recalculate the optimal state at k and the optimal state covariance at k.

[0073] The effects and benefits of the present invention are:

[0074] (1) Wide applicability: The present invention only needs to obtain the pitch and roll angles of the target object, and low-precision inertial sensors can meet this requirement. The algorithm is based on the iterative solution of the unscented Kalman filter, with low computational complexity and low computing power requirements, and is applicable to a wide range of environments. The present invention does not restrict the arrangement of visible light feature points, and only requires three feature points. It is applicable to a wide range of scenarios and can be easily deployed in complex environments.

[0075] (2) Strong stability: Compared with other visible light positioning solutions, the present invention is insensitive to the coordinate recognition error of visible light feature points in the camera's pixel image, and can locate at a longer distance. It is not only suitable for indoor environments, but also for complex outdoor environments such as city streets, mountainous areas, and oceans. BRIEF DESCRIPTION OF THE DRAWINGS

[0076] Figure 1 It is a scene model implemented by the present invention.

[0077] Figure 2 Describes the three attitude angles of the target object.

[0078] Figure 3 Flowchart of the present invention. DETAILED DESCRIPTION

[0079] The specific implementation of the present invention is described in detail below in conjunction with the technical solution (and drawings).

[0080] A binocular positioning method with unconstrained light source arrangement is proposed. First, a process equation is established in the framework of the unscented Kalman filter algorithm to predict the position and attitude angle of the target object in three-dimensional space. Then, a measurement equation is established based on the pixel image of the binocular camera to correct the visible light feature point position and heading angle of the target object calculated in the process equation.

[0081] Figure 1 It is a scene model implemented by the present invention. Figure 1 At any k moment in time, camera 1 and camera 2 form a binocular camera, O 1 and O 2 The optical centers of the lenses of camera 1 and camera 2 are O 1 and O 2 As the origin, the lens is outward in the positive direction of the z axis to establish two three-dimensional right-handed coordinate systems, namely CCS1 and CCS2; the pixel images of camera 1 and camera 2 are in the positive direction of the z axis of the binocular camera and are aligned with xO i y(i=1,2) are parallel, and the intersection of the z axis of CCS1 and CCS2 with their respective pixel images is the center point (u0,v0) of the pixel image. i,k The target object (i=1, 2, 3) moves in three-dimensional space. The three visible light feature points are captured by the binocular cameras and imaged into two pixel images. Their coordinates on the pixel images of camera 1 and camera 2 are P i pj (i=1,2,3;j=1,2). Figure 2The three attitude angles of the target object are described. The object coordinate system (OCS) is established with the target object's center of mass as the origin and parallel to the axes of CCS1. The three attitude angles include pitch (α), heading (β), and roll (γ), which represent the target object's rotation angles around the z-, y-, and x-axes of the OCS, respectively.

[0082] Figure 3 The flowchart of the present invention is as follows:

[0083] Step 1: For the target object motion process of three visible light feature points, establish the unscented Kalman filter state vector, process equation, and measurement equation;

[0084] Step 2: Binocular camera calibration;

[0085] Step 3: Initialize the parameters of the unscented Kalman filter;

[0086] Step 4: For any moving target object in the kth calculation cycle, the pitch angle and roll angle are collected by the six-axis inertial sensor, and the positions of the three visible light feature points in the pixel images of camera 1 and camera 2 are collected by the binocular camera;

[0087] Step 5: Iterate the unscented Kalman filter: sequentially calculate the state sigma point set, process sigma point set, state covariance matrix, measurement sigma point set, measurement covariance matrix, Kalman gain, optimal state, and covariance matrix of the optimal state for the kth calculation cycle, and finally output the position and heading angle of the three visible light feature points of the target object from the optimal state;

[0088] Step 6: Let the current calculation cycle be the k-1th calculation cycle, the next calculation cycle be the kth calculation cycle, return to step 4, and perform calculations for the kth calculation cycle again.

[0089] The above-described embodiments merely express the implementation methods of the present invention, but should not be understood as limiting the scope of the patent of the present invention. It should be pointed out that for those skilled in the art, several variations and improvements can be made without departing from the concept of the present invention, and these all fall within the scope of protection of the present invention.

Claims

1. A binocular positioning method with unconstrained arrangement of light sources, characterized in that: The method is a visible light positioning method that uses an unscented Kalman filter to fuse inertial sensor data and binocular camera data. First, within the unscented Kalman filter algorithm framework, a process equation is established to predict the position and attitude angle of a target object in three-dimensional space. Then, a measurement equation is established based on the pixel image of the binocular camera to correct the position and heading angle of the visible light feature points of the target object calculated in the process equation. The method includes the following steps: Step 1. Initialization Step 1.1 Camera calibration Let the left and right cameras of the binocular camera be camera 1 and camera 2 respectively; perform binocular camera calibration on camera 1 and camera 2 to obtain the intrinsic and extrinsic parameters of the two cameras respectively; the intrinsic parameters include focal length f, pixel image center coordinates (u0, v0), pixel size dx, dy, and the intrinsic parameters of camera 1 and camera 2 are consistent; the extrinsic parameters include the rotation matrix R of camera 2 relative to camera 1 CCS , CCS represents the camera coordinate system, translation matrix T CCS ; Step 1.2 Establish the state vector of the unscented Kalman filter With the optical center of the lens of camera 1 as the origin and the outward direction of the lens as the positive direction of the z-axis, a right-handed three-dimensional coordinate system is established. This coordinate system is camera coordinate system 1 and is defined as CCS1. Similarly, camera coordinate system 2 is established on camera 2 and is defined as CCS2. Let CCS1 be the main coordinate system; let the predicted state χ of the kth cycle of the calculation cycle be k =[P 1,k P 2,k P 3,k T k β k ] T , the kth period is called k moment, the state vector is called state, where P i,k (i=1,2,3) is the coordinate of the i-th visible light feature point under CCS1 when it moves with the target object at the k-th moment; T k Denote the translation vector at time k; β k is the heading angle at time k; When k = 0, it is the initial state of the unscented Kalman filter, which can be set according to the distance and relative posture between the current binocular camera and the target object; Step 1.3 Establish the process equation of the unscented Kalman filter First, the state vector χ at time k k Establish the following equation: The left side of the equation is the state at time k; the right side of the equation is the equation for calculating the state at time k using the state at time k-1, P i,k-1 is the coordinate of the i-th visible light feature point in the three-dimensional space moving with the target object under CCS1 at time k-1; T k-1 Denote the translation vector at time k-1; R k Denote the rotation matrix at time k, through the pitch angle α at time k k and the roll angle γ k And the pitch angle at time k-1 is calculated using the following formula: This formula is the Euler-Rodriguez formula; using Calculate the average value of the translation vector output of the first m cycles at time k to calculate T k-1 Similarly, use Calculate the average value of the heading angle output of the first m cycles at time k to calculate β k-1 ;w k-1 is the noise of the predicted state at time k-1, which is automatically estimated by the unscented Kalman filter algorithm; The above equations are collectively called process equations χ k =F(χ k-1 ,α k ,γ k ), which describes how to use the state χ at time k-1 k-1 Predict the state χ at time k k ; Step 1.4 Establish the measurement equation of the unscented Kalman filter The camera images any point in the three-dimensional space onto the camera's pixel image. If the positional relationship between the coordinates of any point in the three-dimensional space and the camera's pixel image can be obtained, then after the coordinates of the point on the camera's pixel image are known, partial information about the coordinates of the point in the three-dimensional space can be obtained. This information and the Kalman filter algorithm can be used to predict the state χ in step 1.

2. k Make corrections, a process called measurement; The binocular camera has been calibrated in step 1.1, so: This matrix A is called the internal parameter matrix; For the kth moment, the coordinates P of the i-th feature point under CCS1 i,k Change to augmented form P i,k =(x i,k ,y i,k ,z i,k ,1) T , the pixel coordinates of the point in the camera 1 image The superscript p1 is the pixel image of camera 1, is the horizontal and vertical coordinates of the point in the pixel image; if i=1,2,3, then P i,k and The relationship is: Write this equation as: Where E3 is the third-order unit matrix; at the same time, we also need to obtain P i,k Coordinates of the pixel image of camera 2 The superscript p2 is the pixel image of camera 2. First calculate P i,k Coordinates under CCS2 As follows, where i = 1, 2, 3: but for: The formula is recorded as: Finally, the equation and Merge into: This equation is the measurement equation. The measurement equation converts the point coordinates P under CCS1 into i,k Converted into the pixel image coordinates of camera 1 and camera 2 of the binocular camera, that is, the predicted state χ in step 1.2 can be obtained k P in i,k Calculate the pixel image coordinates of camera 1 and camera 2; obtain from the binocular camera And obtain R from the binocular camera calibration results CCS 、T CCS Then, the unscented Kalman filter is used to filter P i,k Make corrections; Step 1.5 Initialization parameters of the unscented Kalman filter In steps 1.2, 1.3, and 1.4, the Kalman filters for the state, process equation, and measurement equation are obtained. The initial values ​​of the unscented Kalman filter must also be determined. These initial values ​​are the starting position and attitude of the target object. The process noise covariance matrix S must be set based on the accuracy of the process equation in describing the changes in the coordinates and attitude angles of the three feature points during the actual target object's motion. Correspondingly, the measurement noise covariance matrix L must be set based on the accuracy of the measurement equation in describing the camera imaging. Step 2. Unscented Kalman filter iteration Step 2.1 Obtaining pitch and roll angles For a target object with three unconstrained visible light feature points, a six-axis inertial sensor is installed to obtain three real-time changing attitude angles during the three-dimensional space movement, namely: pitch angle, heading angle, and roll angle. The three attitude angles at any k-th moment are recorded as: α k , β k , γ k ; Step 2.2 Image acquisition For a target object with three unconstrained visible light feature points, a binocular camera is used to capture the positions of the three visible light feature points in the camera pixel image during its movement. The coordinates of the i-th visible light feature point at any k-th time in the pixel image of the j-th camera are marked as P i pj , i=1,2,3; j=1,2; the superscript pj is the pixel image of the jth camera; Step 2.3 Calculate the sigma point set of the state at time k-1 The optimal state X at time k-1 k-1 is dispersed into 2n+1 state sigma point sets according to the following rules: χ l,k-1 Then it is the optimal state X at time k-1 k-1 The sigma point set of the state at time k-1, where l = 0, 1, ..., 2n; n is X k-1 The number of members; λ is the diagonal factor, which is a constant and is calculated as λ=ε 2 (n+κ)-n, ε is the distribution range factor, which is a constant, κ is the distribution factor, which is a constant; Q k-1 is the optimal state covariance matrix at time k-1, which is automatically estimated by the unscented Kalman filter; Step 2.4 Calculate the sigma point set of the k-time process In step 1.3, the process equation χ was established. k =F(χ k-1 ,α k ,γ k ), the sigma point set χ of the k-1 moment state in step 2.3 l,k-1 The sigma point set of the state calculated by this equation is updated in the process equation, which is the sigma point set χ of the process at time k l,k , where l = 0, 1, ..., 2n: Where W l m is the mean weight vector, where l=0,1,...,2n, and the calculation formula is: is the sigma point set χ of the process l,k In the mean weight vector W l m The mean of the sigma point set of the process under; Step 2.5 Calculate the state covariance matrix at time k is the state covariance matrix at time k; W l c is the variance weight vector, where l=0,1,...,2n, and the calculation formula is: S is the process noise covariance matrix, set in step 1.5; Step 2.6 Calculate the sigma point set of the measurement at time k In step 1.4, the measurement equation H(A,P i,k ,R CCS ,T CCS ), the sigma point set χ of the k-time process in step 2.4 l,k Substitute this equation to calculate the update of the point set in the measurement equation and obtain the sigma point set measured at time k, where l = 0, 2, ..., 2n: Z l,k is the measurement sigma point set at time k, is the mean weight vector W at time k l m The mean of the process sigma point set under , where l = 0, 1, ..., 2n; Step 2.7 Calculate the measurement covariance matrix at time k is the measurement covariance matrix at time k; L is the measurement noise covariance matrix, which is set in step 1.5; Step 2.8 Calculate the Kalman gain at time k is the state-measurement joint covariance matrix at time k; K k is the Kalman gain at time k; Step 2.9 Calculate the optimal state at time k and repeat the iteration Step 2.9.1 Calculate the optimal state at time k X k is the optimal state at time k; z k is the pixel coordinate of the visible light feature point at time k obtained from step 2.2 Where i = 1, 2, 3; j = 1, 2; Q k is the covariance matrix of the optimal state at time k; Step 2.9.2 Output and Repeat Iteration After calculating the optimal state at time k and the optimal state covariance at time k in step 2.9.1, the iteration at time k is completed, X k That is, the coordinates and attitude angles of the three visible light feature points of the target object under CCS1; let this k moment be k-1 moment, the next moment be k moment, return to step 2.1 to recalculate the optimal state at k moment and the optimal state covariance at k moment.

Images