CN114370867A - Attitude optimization method of high-dynamic star sensor - Google Patents
Attitude optimization method of high-dynamic star sensor Download PDFInfo
- Publication number
- CN114370867A CN114370867A CN202210025953.8A CN202210025953A CN114370867A CN 114370867 A CN114370867 A CN 114370867A CN 202210025953 A CN202210025953 A CN 202210025953A CN 114370867 A CN114370867 A CN 114370867A
- Authority
- CN
- China
- Prior art keywords
- star sensor
- attitude
- dynamic star
- state
- covariance matrix
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 38
- 238000005457 optimization Methods 0.000 title claims abstract description 7
- 239000013598 vector Substances 0.000 claims abstract description 57
- 239000011159 matrix material Substances 0.000 claims abstract description 54
- 238000005259 measurement Methods 0.000 claims abstract description 35
- 238000001914 filtration Methods 0.000 claims abstract description 25
- 230000036544 posture Effects 0.000 claims description 9
- 239000000126 substance Substances 0.000 claims description 9
- 238000012546 transfer Methods 0.000 claims description 9
- FGUUSXIOTUKUDN-IBGZPJMESA-N C1(=CC=CC=C1)N1C2=C(NC([C@H](C1)NC=1OC(=NN=1)C1=CC=CC=C1)=O)C=CC=C2 Chemical compound C1(=CC=CC=C1)N1C2=C(NC([C@H](C1)NC=1OC(=NN=1)C1=CC=CC=C1)=O)C=CC=C2 FGUUSXIOTUKUDN-IBGZPJMESA-N 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000000694 effects Effects 0.000 claims description 4
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 238000002474 experimental method Methods 0.000 claims description 3
- 230000000644 propagated effect Effects 0.000 claims description 3
- 230000001902 propagating effect Effects 0.000 claims description 3
- 238000000691 measurement method Methods 0.000 abstract 1
- 238000011160 research Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/02—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by astronomical means
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
- G06F17/12—Simultaneous equations, e.g. systems of linear equations
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Computational Mathematics (AREA)
- Mathematical Analysis (AREA)
- Theoretical Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Computing Systems (AREA)
- Automation & Control Theory (AREA)
- Astronomy & Astrophysics (AREA)
- Operations Research (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Navigation (AREA)
Abstract
The invention relates to a method for optimizing the attitude of a high-dynamic star sensor, which comprises the following steps: a. analyzing the change relation of the attitude of the high-dynamic star sensor along with time when the high-dynamic star sensor works; b. analyzing the change relation of the attitude of the high-dynamic star sensor along with time when the high-dynamic star sensor works, and modeling the high-dynamic star sensor into a nonlinear control system; c. initializing parameters in the modeled nonlinear control system; d. predicting a state vector in the nonlinear control system and a square root of a state error covariance matrix; e. correcting the predicted state vector in the nonlinear control system and the square root of the state error covariance matrix; f. and d, repeating the step d and the step e by taking the corrected state vector and the state error covariance matrix as initial values so as to realize iterative filtering of the measured attitude of the high-dynamic star sensor. The attitude measurement method can effectively improve the attitude measurement precision of the high-dynamic star sensor and realize the attitude optimization of the high-dynamic star sensor.
Description
Technical Field
The invention relates to an attitude optimization method of a high-dynamic star sensor.
Background
The star sensor is an attitude measuring instrument taking fixed stars as measuring objects, has the characteristics of light weight, small volume, low power consumption, high precision, strong anti-interference performance, capability of autonomous navigation without depending on other systems and the like, and is widely applied to attitude measurement of various space vehicles at present. The attitude measurement principle of the star sensor is as follows: firstly, imaging a fixed star by using an optical lens and an image sensor, obtaining the position and brightness information of the star point on the image sensor through star point extraction and mass center positioning, then finding the corresponding fixed star of the star point in a star table through star map identification, finally obtaining the three-axis attitude of the star sensor through attitude calculation according to the identification result, and providing attitude data for a carrier control system to realize the navigation of a carrier. Generally, the star sensor works in the stage of stable flight of a carrier, when the star sensor works in high-dynamic fields such as initial track entering of the carrier, maneuvering, large-angle attitude adjustment and the like, a star point moves in a photosensitive area of an image sensor within exposure time, and finally a section of track image is formed on a target surface of the sensor, so that the accuracy of star point centroid positioning is reduced, and the attitude measurement accuracy of the star sensor is influenced. Therefore, how to improve the attitude measurement accuracy of the high-dynamic star sensor is an urgent problem to be solved in the current star sensor research.
At present, many research institutions at home and abroad have proposed methods for improving the attitude measurement accuracy of the high-dynamic star sensor, and the methods are mainly divided into two types: one is a method based on extended kalman filtering: although the method solves the problem of attitude filtering of the high-dynamic star sensor, truncation errors are caused after expanded high-order terms are ignored, and the accuracy of attitude filtering is influenced; another class is unscented kalman filter based approaches: the method adopts the unscented transformation to ensure higher attitude filtering precision, but is easily influenced by system parameters to cause filtering divergence and has poorer robustness.
Disclosure of Invention
In view of the above, it is necessary to provide a method for optimizing the attitude of a high dynamic star sensor.
The invention provides a method for optimizing the attitude of a high-dynamic star sensor, which comprises the following steps: a. analyzing the change relation of the attitude of the high-dynamic star sensor along with time when the high-dynamic star sensor works; b. modeling the posture of the high-dynamic star sensor into a nonlinear control system by analyzing the change relation of the posture of the high-dynamic star sensor along with time when the high-dynamic star sensor works; c. initializing parameters in the modeled nonlinear control system; d. predicting a state vector in the nonlinear control system and a square root of a state error covariance matrix by square root cubature Kalman filtering; e. correcting the predicted state vector in the nonlinear control system and the square root of a state error covariance matrix by utilizing square root cubature Kalman filtering; f. and d, repeating the step d and the step e by taking the corrected state vector and the state error covariance matrix as initial values to realize iterative filtering of the measured attitude of the high-dynamic star sensor, thereby finishing the attitude optimization of the high-dynamic star sensor.
Specifically, the method further comprises the steps of:
and verifying the filtering effect through experiments.
Specifically, the step a specifically includes:
quaternion q ═ q for star sensor posture0 q1 q2 q3]TTo describe, let w ═ wx wy wz]TThe angular velocity vector of the high-dynamic star sensor is obtained, and the change of the attitude along with time meets the following conditions:
solving the differential equation in the formula (1) to obtain q (t) which changes with time and satisfies:
q(t)=eW·t (2)
let Δ t be tk-tk-1Obtaining q (t) according to the formula (2)k) And q (t)k-1) In betweenThe relationship is as follows:
q(tk)=eW·Δtq(tk-1) (3)
specifically, the step b specifically includes:
the state equation and the measurement equation of the nonlinear control system are as follows:
wherein: x is a state vector of the system and represents the attitude of the high-dynamic star sensor; z is a measurement vector of the system and represents a measurement value of the attitude of the high-dynamic star sensor; w represents gaussian noise with mean 0 and covariance matrix Q; v represents gaussian noise with mean 0 and covariance matrix R; phi is ak-1Is a state transfer function of the system, hkIs a measurement function of the system;
according to the formula (3) and the formula (4), the state transfer function phi of the systemk-1Expressed as:
in addition, because the state vector and the measurement vector of the system are the postures of the high-dynamic star sensor, the measurement function h of the systemk(xk)=xk。
Specifically, the step c comprises:
estimation of system state vectorSquare root of sum state error covariance matrix C0And (3) initializing:
where ' E ' represents the mathematical expectation, the function chol represents the decomposition of the matrix using Cholesky's method.
Specifically, the step d includes:
using estimates of state vectorsSquare root of sum state error covariance matrix C0And calculating volume points of the state vector:
wherein a represents the matrix [ I4×4,-I4×4];
Propagating the volume points through the state transfer function and computing the predicted state vector:
the square root of the predicted state error covariance matrix is calculated:
wherein the content of the first and second substances,the function Tria represents triangularization calculation on the matrix.
Specifically, the step e includes:
calculate volume points of the predicted state vector:
the volume points are propagated through the measurement function and the predicted measurement vector is calculated:
the square root of the innovation covariance matrix is calculated:
Czz,k|k-1=Tria([zk|k-1,CR,k]) (13)
calculating a cross covariance matrix of the state vector and the measurement vector:
Pxz,k|k-1=xk|k-1(zk|k-1)T (14)
calculating a gain matrix, modifying the square root of the predicted state vector and the state error covariance matrix:
specifically, the step f includes:
after obtaining the corrected state vectorSquare root of sum state error covariance matrix CkAnd then, repeating the step d and the step f by taking the initial value as the initial value, namely performing iterative filtering on the attitude z measured by the high-dynamic star sensorA wave.
The method carries out system modeling on the high-dynamic star sensor, and predicts and corrects the attitude of the high-dynamic star sensor by square root cubature Kalman filtering on the basis. The method is easy to realize, and the attitude measurement precision of the high-dynamic star sensor is effectively improved.
Drawings
FIG. 1 is a flow chart of a method for optimizing the attitude of a high dynamic star sensor according to the present invention;
fig. 2 is an attitude quaternion representation diagram of the high-dynamic star sensor before and after filtering according to the embodiment of the invention.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and specific embodiments.
Referring to fig. 1, a flow chart of the operation of the attitude optimization method of the high dynamic star sensor according to the preferred embodiment of the invention is shown.
And step S1, analyzing the change relation of the posture of the high-dynamic star sensor along with the time when the high-dynamic star sensor works. Specifically, the method comprises the following steps:
in the present embodiment, the attitude of the star sensor is represented by a quaternion q ═ q0 q1 q2 q3]TA description will be given. Let w be [ w ]xwy wz]TThe angular velocity vector of the high-dynamic star sensor is obtained, and the change of the attitude along with time meets the following conditions:
solving the differential equation in the formula (1) to obtain q (t) which changes with time and satisfies:
q(t)=eW·t (2)
let Δ t be tk-tk-1Obtaining q (t) according to the formula (2)k) And q (t)k-1) The relationship between them is:
q(tk)=eW·Δtq(tk-1) (3)
and step S2, modeling the high-dynamic star sensor into a nonlinear control system by analyzing the change relation of the posture of the high-dynamic star sensor along with time when the high-dynamic star sensor works. Specifically, the method comprises the following steps:
as can be seen from the formula (3) and the formula (4), when the star sensor operates under the high dynamic condition, the attitude of the star sensor changes nonlinearly with time, so that the high dynamic star sensor is modeled into a nonlinear control system, and the present embodiment lists the state equation and the measurement equation of the nonlinear control system:
wherein: x is a state vector of the system and represents the attitude of the high-dynamic star sensor; z is a measurement vector of the system and represents a measurement value of the attitude of the high-dynamic star sensor; w represents gaussian noise with mean 0 and covariance matrix Q; v represents gaussian noise with mean 0 and covariance matrix R; phi is ak-1Is a state transfer function of the system, hkAs a function of the measurements of the system.
According to the formula (3) and the formula (4), the state transfer function phi of the systemk-1Expressed as:
in addition, because the state vector and the measurement vector of the system are the postures of the high-dynamic star sensor, the measurement function h of the systemk(xk)=xk。
And step S3, initializing parameters in the nonlinear control system. Specifically, the method comprises the following steps:
estimation of system state vectorSquare root of sum state error covariance matrix C0And (3) initializing:
where ' E ' represents the mathematical expectation, the function chol represents the decomposition of the matrix using Cholesky's method.
Step S4, using square root cubature kalman filtering, predicting a state vector in the nonlinear control system and a square root of a state error covariance matrix. Specifically, the method comprises the following steps:
using estimates of state vectorsSquare root of sum state error covariance matrix C0And calculating volume points of the state vector:
wherein a represents the matrix [ I4×4,-I4×4]。
Propagating the volume points through the state transfer function and computing the predicted state vector:
the square root of the predicted state error covariance matrix is calculated:
wherein the content of the first and second substances,the function Tria represents triangularization calculation on the matrix.
Step S5, using square root cubature kalman filtering, correcting the state vector predicted in the nonlinear control system and the square root of the state error covariance matrix. Specifically, the method comprises the following steps:
calculate volume points of the predicted state vector:
the volume points are propagated through the measurement function and the predicted measurement vector is calculated:
the square root of the innovation covariance matrix is calculated:
Czz,k|k-1=Tria([zk|k-1,CR,k]) (13)
calculating a cross covariance matrix of the state vector and the measurement vector:
Pxz,k|k-1=xk|k-1(zk|k-1)T (14)
calculating a gain matrix, modifying the square root of the predicted state vector and the state error covariance matrix:
and S6, repeating the step S4 and the step S5 by taking the corrected state vector and the state error covariance matrix as initial values to realize iterative filtering of the attitude measured by the high-dynamic star sensor, and verifying the filtering effect through experiments. Specifically, the method comprises the following steps:
after obtaining the corrected state vectorSquare root of sum state error covariance matrix CkAnd then, repeating the steps S4 and S5 by taking the initial value as the initial value, so that the attitude z measured by the high-dynamic star sensor can be subjected to iterative filtering.
In addition, the method verifies that the attitude measured by the high-dynamic star sensor has errors as shown in fig. 2, and the errors are obviously reduced after the method is used for processing, so that the method has a good filtering effect on the attitude of the high-dynamic star sensor.
According to the method, the high-dynamic star sensor is modeled into a nonlinear control system and is processed by square root cubature Kalman filtering, and therefore the attitude measurement precision of the high-dynamic star sensor is effectively improved.
Although the present invention has been described with reference to the presently preferred embodiments, it will be understood by those skilled in the art that the foregoing description is illustrative only and is not intended to limit the scope of the invention, as claimed.
Claims (8)
1. A method for optimizing the attitude of a high-dynamic star sensor is characterized by comprising the following steps:
a. analyzing the change relation of the attitude of the high-dynamic star sensor along with time when the high-dynamic star sensor works;
b. modeling the posture of the high-dynamic star sensor into a nonlinear control system by analyzing the change relation of the posture of the high-dynamic star sensor along with time when the high-dynamic star sensor works;
c. initializing parameters in the modeled nonlinear control system;
d. predicting a state vector in the nonlinear control system and a square root of a state error covariance matrix by square root cubature Kalman filtering;
e. correcting the predicted state vector in the nonlinear control system and the square root of a state error covariance matrix by utilizing square root cubature Kalman filtering;
f. and d, repeating the step d and the step e by taking the corrected state vector and the state error covariance matrix as initial values to realize iterative filtering of the measured attitude of the high-dynamic star sensor, thereby finishing the attitude optimization of the high-dynamic star sensor.
2. The method of claim 1, wherein the method further comprises the steps of:
and verifying the filtering effect through experiments.
3. The method according to claim 2, wherein the step a specifically comprises:
quaternion q ═ q for star sensor posture0 q1 q2 q3]TTo describe, let w ═ wx wy wz]TThe angular velocity vector of the high-dynamic star sensor is obtained, and the change of the attitude along with time meets the following conditions:
solving the differential equation in the formula (1) to obtain q (t) which changes with time and satisfies:
q(t)=eW·t (2)
let Δ t be tk-tk-1Obtaining q (t) according to the formula (2)k) And q (t)k-1) The relationship between them is:
q(tk)=eW·Δtq(tk-1) (3)
4. the method according to claim 3, wherein said step b comprises in particular:
the state equation and the measurement equation of the nonlinear control system are as follows:
wherein: x is a state vector of the system and represents the attitude of the high-dynamic star sensor; z is a measurement vector of the system and represents a measurement value of the attitude of the high-dynamic star sensor; w represents gaussian noise with mean 0 and covariance matrix Q; v represents gaussian noise with mean 0 and covariance matrix R; phi is ak-1Is a state transfer function of the system, hkIs a measurement function of the system;
according to the formula (3) and the formula (4), the state transfer function phi of the systemk-1Expressed as:
in addition, because the state vector and the measurement vector of the system are the postures of the high-dynamic star sensor, the measurement function h of the systemk(xk)=xk。
5. The method of claim 4, wherein said step c comprises:
estimation of system state vectorSquare root of sum state error covariance matrix C0And (3) initializing:
where ' E ' represents the mathematical expectation, the function chol represents the decomposition of the matrix using Cholesky's method.
6. The method of claim 5, wherein step d comprises:
using estimates of state vectorsSquare root of sum state error covariance matrix C0And calculating volume points of the state vector:
wherein a represents the matrix [ I4×4,-I4×4];
Propagating the volume points through the state transfer function and computing the predicted state vector:
the square root of the predicted state error covariance matrix is calculated:
7. The method of claim 6, wherein step e comprises:
calculate volume points of the predicted state vector:
the volume points are propagated through the measurement function and the predicted measurement vector is calculated:
the square root of the innovation covariance matrix is calculated:
Czz,k|k-1=Tria([zk|k-1,CR,k]) (13)
calculating a cross covariance matrix of the state vector and the measurement vector:
Pxz,k|k-1=xk|k-1(zk|k-1)T (14)
calculating a gain matrix, modifying the square root of the predicted state vector and the state error covariance matrix:
8. the method of claim 7, wherein step f comprises:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210025953.8A CN114370867A (en) | 2022-01-11 | 2022-01-11 | Attitude optimization method of high-dynamic star sensor |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210025953.8A CN114370867A (en) | 2022-01-11 | 2022-01-11 | Attitude optimization method of high-dynamic star sensor |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114370867A true CN114370867A (en) | 2022-04-19 |
Family
ID=81144811
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210025953.8A Pending CN114370867A (en) | 2022-01-11 | 2022-01-11 | Attitude optimization method of high-dynamic star sensor |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114370867A (en) |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108344409A (en) * | 2017-12-26 | 2018-07-31 | 中国人民解放军国防科技大学 | Method for improving satellite attitude determination precision |
CN109211276A (en) * | 2018-10-30 | 2019-01-15 | 东南大学 | SINS Initial Alignment Method based on GPR Yu improved SRCKF |
-
2022
- 2022-01-11 CN CN202210025953.8A patent/CN114370867A/en active Pending
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108344409A (en) * | 2017-12-26 | 2018-07-31 | 中国人民解放军国防科技大学 | Method for improving satellite attitude determination precision |
CN109211276A (en) * | 2018-10-30 | 2019-01-15 | 东南大学 | SINS Initial Alignment Method based on GPR Yu improved SRCKF |
Non-Patent Citations (2)
Title |
---|
WANG JUN 等: "An attitude tracking method for star sensor under dynamic conditions", OPTOELECTRONICS LETTERS, vol. 15, no. 5, XP036899427, DOI: 10.1007/s11801-019-8197-z * |
王军: "高动态星敏感器关键技术研究", 中国博士学位论文全文数据库 工程科技Ⅱ辑, pages 5 * |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US5051751A (en) | Method of Kalman filtering for estimating the position and velocity of a tracked object | |
CN111238467B (en) | Bionic polarized light assisted unmanned combat aircraft autonomous navigation method | |
CN105190237A (en) | Heading confidence interval estimation | |
CN105136145A (en) | Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method | |
CN112562077B (en) | Pedestrian indoor positioning method integrating PDR and priori map | |
US20230366680A1 (en) | Initialization method, device, medium and electronic equipment of integrated navigation system | |
CN112683269B (en) | MARG attitude calculation method with motion acceleration compensation | |
WO2022160391A1 (en) | Magnetometer information assisted mems gyroscope calibration method and calibration system | |
CN111189442A (en) | Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF | |
CN111189454A (en) | Unmanned vehicle SLAM navigation method based on rank Kalman filtering | |
CN111649747A (en) | IMU-based adaptive EKF attitude measurement improvement method | |
CN116222551A (en) | Underwater navigation method and device integrating multiple data | |
Pourtakdoust et al. | An adaptive unscented Kalman filter for quaternion‐based orientation estimation in low‐cost AHRS | |
CN112284388B (en) | Unmanned aerial vehicle multisource information fusion navigation method | |
Medewar et al. | A comparison between nonlinear estimation based algorithms for mobile robot localizations | |
CN113587926A (en) | Spacecraft space autonomous rendezvous and docking relative navigation method | |
CN111982126B (en) | Design method of full-source BeiDou/SINS elastic state observer model | |
CN110375773B (en) | Attitude initialization method for MEMS inertial navigation system | |
CN110672127B (en) | Real-time calibration method for array type MEMS magnetic sensor | |
CN110736459B (en) | Angular deformation measurement error evaluation method for inertial quantity matching alignment | |
CN117387604A (en) | Positioning and mapping method and system based on 4D millimeter wave radar and IMU fusion | |
CN114858166B (en) | IMU attitude resolving method based on maximum correlation entropy Kalman filter | |
CN114370867A (en) | Attitude optimization method of high-dynamic star sensor | |
Khoder et al. | A quaternion scaled unscented kalman estimator for inertial navigation states determination using ins/gps/magnetometer fusion | |
CN112712107B (en) | Optimization-based vision and laser SLAM fusion positioning 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 |