基于PnP和自适应线性卡尔曼滤波的实时位姿估计

针对实时位姿估计中扩展卡尔曼滤波(EKF)线性化引入非线性误差和依赖已知噪声分布的缺点,提出一种基于PnP的自适应线性卡尔曼滤波位姿估计求解方法。将PnP位姿估计求解策略引入卡尔曼滤波观测方程,通过对动态方程误差统计参数实时估计,自适应调节卡尔曼滤波递推参数。所提算法求解精度高,固定了观测方程的观测向量维度,提高了算法实用性。通过仿真试验,比较了该算法与EKF的位姿估计精度,通过量化误差分析,证明了该方法可以提高三维运动位姿估计精度,也验证了该方法的有效性。

RealTime Pose Estimation Based on PnP and Adaptive Linear Kalman Filter

Aimed at problems of extended Kalman filter (EKF) linearization exists nonlinear error and dependence on known noise distribution in realtime pose estimation, an adaptive linear Kalman filtering based on PnP for pose estimation was proposed. The PnP pose estimation solution strategy was introduced into the Kalman filter observation equation, and the Kalman filter recursive parameters were adaptively adjusted by realtime estimation of the statistical error parameters of the dynamic equation. The proposed algorithm had high accuracy, fixed the observation vector dimension of the observation equation, and improves the practicality of the algorithm. Through simulation experiments, the accuracy of pose estimation of this algorithm and EKF was compared. Through quantization error analysis, it was proved that this method can improve the precision of threedimensional motion pose estimation and verified the effectiveness of the method.