Research On Observability Analysis And Dynamics Filter Algorithms Of Monocular Camera/INS Integrated Navigation

Vision/SINS integrated navigation is an important topic in the integrated navigation,and has attracted considerable attentions from the civil and the military area. In order toestimate precise state of vision/SINS integrated navigation system, observability analysismust be done after filter design. Since vision/SINS integrated navigation system is a non-linear system, observability analysis is necessary when the system with linear and angulardynamics. Most of visual navigation algorithms need observations of known features. Itishardtosatisfyintheunknownenvironment. Motivatedbytheabove-mentionedinsuffi-ciencies, the main purpose of this dissertation is to address two important and challengingissues of monocular camera/SINS integrated navigation in the unknown environment:observability analysis and dynamic filter algorithms. The main contributions include thefollowing aspects.1. The estimate of position and orientation of moving object from monocular camerawas investigated. The EKF was designed according to Goddard method. Some mistakesin Goddard method were rectified. At the same time, relative translation and rotationsecondderivations of the object were added as state assignment. Comparedwith Goddardmethod, simulated data and actual experimental data show that the improved method canget better precision and robustness.2. The insufficiency of monocular vision navigation was analyzed. The improvedmethod still cannot estimate precise state when the system with high dynamics or overlong distance. It need combine with other sensors, such as SINS.3. For the monocular camera/high accuracy IMU integrated navigation system, EKFis used to estimate the error state. Under the condition of the camera positioning height isknown,observabilityoftheerrorsofattitudeandvelocityisprovedbyPWCSmethod. Forpoint feature, the observability of attitude and velocity errors need at least there featuresare detected on the ground. For line feature, the observability need at least two linearlyindependent features are detected on the ground.4. The EKF algorithm is proposed based on point and line feature, respectively. Forpoint feature, velocity integrated mode is used in the integrated navigation. With the helpof SINS, camera isn’t need to estimate orientation. SINS provide orientation with cameracontinuously. Camera’s velocity is solved after sensor alignment and time synchroniza-tion, and used to amend SINS’s velocity and position by Kalman filter. Integrated nav- igation calibrates camera scale factor on-line. Compared with visual navigation methodof periodic updates to the orientation estimate, the proposed method is more accurate andmore robustness, low rate of error growth. For line feature, dual quaternion was intro-duced to describe the position and attitude of camera. The proposed algorithm deducedthe increment formula between image feature and camera position. The velocity compu-tation difference between the SINS and the camera is chosen as observation of integratednavigation. A Kalman filter is used to correct the integrated navigation error includingthe camera scale factor. The experiment results show the proposed algorithm is accuratefor structural road.5. For the monocular camera/low accuracy IMU/magnetometer integrated naviga-tion system, the matrix Kalman filter(MKF) is used to estimate the state. Under the con-dition of the camera positioning height is known and gyro biases and magnetic variationsneed to be estimated, observability analysis is directly based on nonlinear system withLie derivatives. For point feature, the observability of attitude and velocity need at leastthere features are detected on the ground. At least one degree of rotational freedom isexcited. For line feature, the observability need at least two linearly independent featuresare detected on the ground. At least one degree of rotational freedom is excited. Theobservability conditions of attitude and velocity are equivalent for point and line feature.Compared with visual navigation, observability conditions of monocular camera/IMU in-tegrated navigation need less features. At the same time, the position of feature is no longneed to be known. There are valuable for the filter design and realization of the integratednavigation system.6. The MKF algorithm is proposed based on point and line feature, respectively. Itis designed such that the estimate of the state matrix is expressed in terms of the matrixparametersoftheoriginalplant. TheMKFpreservesthenaturalformulationoftheattitudematrixtorearrangetheoriginalnonlinearprocessmodelinapseudo-linearprocessmodel.