A Closed-Form Solution for Estimating the Accuracy of Circular Feature’ Pose for Object 2D-3D Pose Estimation System

Algorithm 2

  UKF for object 2D-3D pose estimation using circular feature.

	Initialize: the state is a random vector with known mean and covariance , indicates the process noise;
	Set selection of sigma points:
(1)	A set of sigma points: , where is the dimension of the state vector;
(2)	The associated weights of each sigma points: , , and the weights and are used to compute the mean and covariance, respectively. There are some suggestions for the values of some parameters: , , ,
	Prediction process:, where the index denotes the number of columns in the matrix ;
	Resampling of sigma points:;
	Observation prediction:, , where indicates the observation noise;
	The cross covariance:;
	The gainis given by:;
	Update: the posterior state vector and covariance are updated after the following formula:
	, , where is the arrived observation in the step time .