# Monocular Vision-Based Robot Localization and Target Tracking

Authors: Bing-Fei Wu; Wang-Chuan Lu; Cheng-Lung Jen
Journal: Journal of Robotics
 (2011)

---

## Abstract

This paper presents a vision-based technology for localizing targets in 3D environment. It is achieved by the combination of different types of sensors including optical wheel encoders, an electrical compass, and visual observations with a single camera. Based on the robot motion model and image sequences, extended Kalman filter is applied to estimate target locations and the robot pose simultaneously. The proposed localization system is applicable in practice because it is not necessary to have the initializing setting regarding starting the system from artificial landmarks of known size. The technique is especially suitable for navigation and target tracing for an indoor robot and has a high potential extension to surveillance and monitoring for Unmanned Aerial Vehicles with aerial odometry sensors. The experimental results present “cm” level accuracy of the localization of the targets in indoor environment under a high-speed robot movement.


---

## Body

## 1. Introduction

Knowledge about the environment is a critical issue for autonomous vehicle operations. The capability of localizing targets with a robot for environment is highly demanded. In [1], it investigates the vision-based object recognition technique for detecting targets of the environment which have to be reached by a robot. After the robot finishes the target detection, it is an essential task for robots to know the positions of targets. The tasks include navigation and object tracking. As a result, the approach to localize targets for specific tasks is an essential issue in some applications. For UAVs, there are some similar demands [2, 3] that UAVs have to track the positions of ground objects for reconnaissance or rescue assistance with monocular vision. Besides, UAVs need to observe the changing surroundings to understand the movements of the aircraft better, or the localizing system has to direct the aircraft to a region of interest after taking ground observations.In recent years, odometry sensors have been widely used for estimating the motion of vehicles moving in a 3D space environment such as UAVs and Unmanned Ground Vehicles (UGVs). For instance, Inertial Navigation Systems (INSs) are applied to measure linear acceleration and rotational velocity, and capable of tracking the position, velocity and attitude of a vehicle by integrating these signals [4] or mobile vehicles use the Global Position System (GPS) and Inertial Measurement Units (IMUs) for land vehicle applications [5]. However, most of inertial navigation system sensors are expensive for some applications in the indoor environment. Optical wheel encoders and an electrical compass provide linear and angular velocities respectively. Both of two sensors are basic odometry sensors and widely used owing to their low cost, simplicity, and easy maintenance. Encoders provide a way of measuring the velocity to estimate the position of the robot and compasses are often used to detect the orientations of the robot. Based on the sensor information, motion control is done and then the localization of the robot is estimated [6, 7]. Despite some limitations of an encoder, most researchers agree that an encoder is a vital part of the robot’s navigation system and the tasks will be simplified if the encoder accuracy is improved. Besides, the additional sensor, a camera, allows a robot to perform a variety of tasks autonomously. The use of computer vision for localization has been investigated for several decades. The camera has not been at the center of robot localization while most of researchers have more attention to other sensors such as laser range-finders and sonar. However, it is surprising that vision is still an attractive choice for sensors because cameras are compact, cheaper, well understood, and ubiquitous. In this paper, the algorithm is achieved by combining different types of sensors including optical wheel encoders, an electrical compass, and a single camera.Mobile vehicles such as robots, UGVs, and UAVs are becoming fully intelligent robotic systems which can handle more complicated tasks and environments. It is necessary to provide them with higher autonomy and better functionalities. For instance, robot localization and target tracking are mandatory. For these tasks, some of sensors such as GPSs, IMUs, laser range-finders, encoders, and compasses are possibly combined to describe the time evolution of the motion model. Additionally, visual sensors, such as electro-optic or infrared cameras, have been included in the on-boarded sensor suit of many robots to increase their value. Therefore, it is a popular research topic regarding how to combine hybrid sensors to achieve a special task. Recent researches have shown that the way to improve sequential estimation and achieve repeatable motion with multisensors is to adopt the probabilistic method. The Kalman filter has been widely used for data fusion such as navigation systems [8], virtual environment tracking systems [9], and 3D scene modeling [10]. The Kalmna filter is a mathematical method to provide an iterated procedure for a least-square estimation of a linear system. The procedure includes predicting state by using the motion model and then correcting the state by the measurement innovation. The EKF is a varied type of the Kalman filter for a non-linear system such as the target tracking system in this paper. More details about the Kalman filter are introduced in the material of [11–14]. In this paper, the algorithm represents a new approach of EKF-based multisensor data fusion. The information from different types of sensors is combined to improve the estimations in the system state. In order to make our system able to deal with the variety of tasks such as navigation for a mobile robot or ground target tracking for UAVs, we are determined to choose monocular vision rather than binocular vision as one type of our sensors. Based on the monocular vision, simultaneous robot localization and target tracking become more complex with a higher computational loading due to unknown depths of targets in only one image observation. Thus, it is an important issue regarding how to make use of computational tricks or adopt more efficient algorithms to reduce the running time. In this paper, Chelesky decomposition [15] and forward-and-back substitution are used to invert the covariance matrix for improving the computational efficiency because the property of covariance matrix is semipositive definite.The rest of this paper is structured as follows. Section2 presents the proposed algorithm and explains the details. Section 3 explains two key points about the proposed algorithm. The experimental results are provided in Section 4, and finally, Section 5 concludes the paper and suggests future work.


## 2. Explanation of the Algorithm

The algorithm is mainly divided into five parts including motion modeling, new target adding, measurement modeling, image matching, and EKF updating. By sequential measurement from sensors, the EKF is capable of improving the initial estimate for the unknown information while simultaneously updating the localization of targets and the robot pose. Finally, Chelesky decomposition and forward-and-back substitution are presented to calculate the inverse covariance matrix efficiently.


### 2.1. The Origins of the Proposed Algorithm

A single camera is mounted on our system as one of the sensors. The monocular vision infers that the depth of the target is not able to be measured by only one image but estimated by the sequential images. Therefore, the single camera has to estimate the depth by observing the target repeatedly to get parallax between different captured rays from the target to the robot. The orientations of target are estimated in the world coordinate system only by one image.Yiw is a six-dimension state vector and used to describe the position of the ith target in 3D space. Its equation is addressed as(1)Yiw=[riwTθiwφiwρiw]T.riw is the location of the robot when the robot detects the ith target in the first time. Gic is defined as the position of the target with respect to the camera coordinate system and denoted by(2)Gic=Rcw((riw-rkw)+1ρiwm(θiw,φiw)).θiw and φiw are the orientations of the ith target and calculated by the pixel location of the target in only one image. The relationship between the position of the target Gic and the current robot localization rkw is presented in Figure 1.Figure 1
The robot moves fromriw to rkw. Based on the two views, the parallax is produced. The initial depth can be refined to approach the real distance and the position of the ith target is estimated.Giw is the position of the target with respect to the world frame system. 1/ρiw is defined as the distance from riw to the target and m(θiw,φiw) is its unit vector. Consequently, 1/ρiwm(θiw,φiw) is seen as the vector from riw to the target and then Giw is calculated as ((riw-rkw)+1/ρiwm(θiw,φiw)). Finally, Gic is estimated if the depth 1/ρiw is known in advance. However, the depth of the target is not able to be measured by a single image. The EKF is applied to estimate 1/ρiw, θiw, and φiw and they converge toward more correct values under the recursive iteration by using sequential image measurement information. To sum up, (2) is the basic concept and origin of our proposed algorithm.


### 2.2. Motion Modeling

We first derive the continuous-time system model that describes the time evolution in the state estimate. This model allows us to employ the sampled measurements of the wheel encoder and the compass for state propagation. The process state is described by the vector(3)Xmk=[rkwTqkwTvkwTωkcT]T,
where rkw is the position of the robot with respect to the world frame. qkw is the quaternion that represents the orientation of the world frame for the robot. The linear velocity in the world frame and the angular velocity with respect to the camera frame are denoted by vkw and ωkc, respectively. In this system, the control inputs are measured by wheel encoders and the compass which provide the linear velocity and the angular velocity, respectively. Besides, the linear velocity is used to simulate the acceleration data of the robot for describing how components of the system state vector change completely. The definition of acceleration is defined as akw=(vkw-vk-1w)/Δt. In order to have similar on-line simulation, we define akw as (vkw-vk-1w)/Δt rather than (vk+1w-vkw)/Δt. Although the former definition is not better than the later one which is closer to the real acceleration, an iterated EKF is still capable of compensating for errors when adopting (vkw-vk-1w)/Δt as the robot’s acceleration. The system model describing the time evolution of the wheel encoders and of the compass is given by(4)Xmk+1-=[rk+1wqk+1wvk+1wωk+1c]=[rkw+vkwΔt+12akwΔt2qkw×q(ωkc+Ωkc)vkw+akwΔtωkc+Ωkc].The compass is used to measureΔθk which is defined as (θk-θk-1). ωk+1c and ωkc are defined as (θk-θk-1)/Δt and (θk-1-θk-2)/Δt, respectively. If ωk+1c is assumed to be (ωkc+Ωkc), Ωkcc is derived as (θk-2θk-1+θk-2)/Δt. qk+1w is denoted by(5)qk+1w=[q1-q2-q3-q4q2q1-q4q3q3q4q1-q2q4-q3q2q1][q1′q2′q3′q4′],
where(6)q(ωkc+Ωkc)=[q1′q2′q3′q4′]=[cos⁡(Δθk2)uxsin⁡(Δθk2)uysin⁡(Δθk2)uzsin⁡(Δθk2)].u=[uxuyuz]Tis equal to ωk+1cΔt/∥ωk+1c∥Δt.The covariance is predicted and modified if considering the control covarianceCov(vkw,ωkc,akw). We assume that the process noise of the control vector is not correlated with the process state vector Xmk so that Cov(Xmk,vkw,ωkc,akw) is set to be a zero matrix. By using (A.4) in Appendix A, the predicted covariance of the system model is expressed as(7)Cov(Xmk+1-)=FXmCov(Xmk)FXmT+Q,
where(8)Q=FvCov(vkw)FvT+FωCov(ωkc)FωT+FaCov(akw)FaT.
The first item FXmCov(Xmk)FXmT represents the noise from the process state vector and Q describes the noise from the measurement of the wheel encoders and the compass. The Jacobian matrix of FXm is shown as(9)FXm=∂f∂Xm=[∂f∂rkw∂f∂qkw∂f∂vkw∂f∂ωkc]13×13.
The Jacobian matrixes of Fv and Fω are as the same as ∂f/∂vkw and ∂f/∂ωkc, respectively.


### 2.3. Adding New Target

It is remarkable about our algorithm that new targets are initialized by using only one image. The initialization includes the state initial values and the covariance assignment.


#### 2.3.1. Target Initialization

Equation (1) is used to define the location of the new target and estimate the six parameters of Yiw with the EKF prediction-update loop. If the robot senses the 1st target at the state k for the first time, the new target information is added and then the process state vector is modified. The expanded process state vector is represented by the following equation:(10)Xk-=[Xmk-Y1w],
where(11)Y1w=[r1wTθ1wφ1wρ1w]6×1T.
The first time observation of the 1st target is done at the current camera location r1w. m(θ1w,φ1w) is defined as an unit vector from the location r1w to the 1st target with respect to the world frame. Figure 2 illustrates the relationship about m(θ1w,φ1w), θ1w, and φ1w. hu=[uv]T is defined as the pixel location of the 1st target in an undistorted image. gc is the location of the 1st target with respect to the camera frame. The location of the 1st target with respect to the world frame gw is addressed as(12)gw=[gxwgywgzw]=Rwcgc=Rwc[(u0-u)Dxf(v0-v)Dyf1]gzc,
where(13)Rwc=[q12+q22-q32-q422(q2q3-q1q4)2(q2q4+q1q3)2(q2q3+q1q4)q12-q22+q32-q422(q3q4-q1q2)2(q2q4-q1q3)2(q3q4+q1q2)q12-q22-q32+q42].
Then we get(14)[gxwgywgzw]gzc=Rwc[(u0-u)Dxf(v0-v)Dyf1].
Only gxw/gzc and gyw/gzc rather than gxw and gyw are computed by using hu. It is impossible to know gzc by only one image. However, it is possible to make use of gxw/gzc and gyw/gzc to calculate θ1w and φ1w which are shown as(15)[θ1wφ1w]=[tan-1⁡(gxwgzc,gzwgzc)tan-1⁡(-gywgzc,(gxwgzc)2+(gzwgzc)2)].
The unit vector m(θ1w,φ1w) is derived as a function of the orientations of the target and described as(16)m(θ1w,φ1w)=[cos⁡φ1wsin⁡θ1w-sin⁡φ1wcos⁡φ1wcos⁡θ1w].
The final parameter 1/ρ1w is not able to be measured by only one image but estimated by the sequential images with EKF prediction-update loop. As a result, it is assumed to be an initial value 1/ρ0.Figure 2
The orientations of the target with respect to the world frame can be estimated as a function of its undistorted pixel locationhu.


#### 2.3.2. New Covariance Assignment

The system covariance is modified after adding a new target. By using (B.1) in Appendix B, the new covariance Cov(Xk-) and the function of Y1w are denoted by(17)Cov(Xk-)=[Cov(Xmk)B1TB1A1],Y1w=k(rkw,qkw,hd(u,v),ρ1w),
where(18)k=[r1wθ1wφ1wρ1w]=[rxryrztan-1⁡(gxwgzc,gzwgzc)tan-1(-gywgzc,⁡(gxwgzc)2+(gzwgzc)2)ρ1w].B1is not a zero matrix because the new target’s location is correlated with the estimates of the robot. According to (B.4) and (B.5) in Appendix B, B1 and A1 are derived as(19)B1=kxCov(Xmk)=[krkq06×306×3],(20)A1=krCov(r)krT+kqCov(q)kqT+a,
where (21)a=khdCov(hd)khdT+kρCov(ρ)kρT.


### 2.4. Measurement Modeling

The robot moves continuously and records the sequential images. This is a process of detecting and identifying the targets. The parameters of targets have been set into the process state vector and the covariance has been estimated with the recursive loop.


#### 2.4.1. Sensor Model

The predicted pixel locations of targets are estimated as a function of the prior process state vector which is described in (4). According to (2), the location of the ith target in the camera frame can be defined in another way:(22)Tic=[Tx(i)cTy(i)cTz(i)c]=Rcw((riw-rkw)*ρiw+m(θiw,φiw)).
Our measurement sensor of the camera is monocular vision. There is a camera sensor model to describe how the sensor maps the variables in the process state vector into the sensor variables. By using the pinhole model [16], Tic is derived as a function of the undistorted pixel location of the ith target (ui,vi) and denoted by(23)Tic=[(u0-ui)Dxf(v0-vi)Dyf1]gz(i)cρiw.
Equation (22) and (23) are combined to derive the predicted pixel location without distortion for the ith target Zk(i)=(ui,vi). Its equation is addressed as(24)Zk(i)=[uivi]=h(Xmk-)=[u0-fDxTx(i)cTz(i)cv0-fDyTy(i)cTz(i)c].
By using image correct, the predicted pixel location of the ith target within distortion Zdk(i)=(ud(i),vd(i)) is addressed as(25)Zdk(i)=hd(Xmk-)=[(ui-u0)1+3k1rd2+5k2rd4(vi-v0)1+3k1rd2+5k2rd4]+[u0v0],
where k1 and k2 are coefficients for the radial distortion of the image.The actual image target the robot takes is the distorted pixel location rather than the undistorted one. Therefore, we have to calculateZdk(i) to get the measurement innovation for EKF updating.


#### 2.4.2. Measurement Covariance Assignment

The measurement covariance is expected to describe what the likely variation of measurement is by the sensor under the current condition. The variation is infected by the variables in the process state vector and the noise which corrupts the sensor. The measurement Jacobian matrixHi is(26)Hi=[∂hd(i)∂Xmk02×6⋯∂hd(i)∂Yiw02×6⋯02×6],
where(27)∂hd(i)∂Xmk=[∂hd(i)∂rkw∂hd(i)∂qkw∂hd(i)∂vkw∂hd(i)∂ωkc]2×13,∂hd(i)∂Yiw=[∂hd(i)∂riw∂hd(i)∂θiw∂hd(i)∂φiw∂hd(i)∂ρiw]2×6.
When there are N targets observed concurrently, they are stacked in one measurement vector Zdk to form a single batch-form update equation which is shown as(28)Zdk=[Zdk(1)T⋯Zdk(N)T]T.
Similarly, the batch measurement Jacobian matrix is derived as(29)Hk=[∂h∂Xmk∂h∂Y1w⋯∂h∂YNw]=[H1T⋯HNT]T,
where(30)Hk=[∂h1∂Xmk2×13∂h1∂Y1w2×602×602×6…02×6∂h2∂Xmk2×1302×6∂h2∂Y2w2×602×6…02×6∂h3∂Xmk2×1302×602×6∂h3∂Y3w2×6…02×6⋮⋮⋮⋮⋱⋮∂hN∂Xmk2×1302×602×602×6…∂hN∂YNw2×6].


### 2.5. Image Matching by Using Cross-Correlation

The image patch information of the targets whose variables are set in the process state vector has been stored in the data-base when the robot senses them in the first time. The predicted pixel locations of the targets within distortion are estimated by using (25). Next important issue is how to search for the actual pixel locations of the targets in a 320 × 240 image. Image matching is a fundamental and vital aspect of many problems in computer vision including motion tracking and object recognition. The image features are invariant to rotation, image scaling, change in illumination, and 3D camera viewpoint. It is still a popular research topic regarding how to allow a single candidate image to be correctly matched with high probability. Based on the estimation of measurement covariance, the small area where the target lies with high probability is able to be predicted. The measurement covariance of the ith target is defined as(31)Cov(Zdk(i))2×2=[σuu(i)2σuv(i)2σvu(i)2σvv(i)2].The template match technique is used to search the actual image pixel location of theith target in the small search area whose width and length are kσuu(i) and kσvv(i), respectively. The coefficient k is a constant and defines how large the search area is. The larger k is, the more is the search time. There are two advantages regarding the search area which is estimated by the measurement covariance. One advantage is that a lot of time is saved because the image pixel location of the ith target is detected in a small search area rather than in a 320 × 240 image. The other one is that the successful search rate increases dramatically because the search area allows the image pixel location to be correctly matched with high probability. As a result, it is not necessary to use a complicated object recognition algorithm such as Scale Invariant Feature Transform (SIFT) [17] for image matching in our system. Cross-correlation search is applied to be our template match algorithm and the computing loading is lower due to its simplification. This approach uses cross-correlation of image to search a suitable image patch. ID is defined as the template image patch for the ith target and stored in the database. It is defined as a candidate image patch in the search area whose width is kσuu(i) and length is kσvv(i). The cross-correlation value of It with ID is given by(32)1M2-1∑uv(ID(u,v)-I̅D)(It(u,v)-I̅t)σIDσIt.M2 is the number of pixels in an image patch and σID and σIt are the stand deviations of ID and ID, respectively. I̅D and I̅t are the average values of ID and It, respectively. The maximum value of cross-correlation of It with ID is the best template matching and it is seen as the matching target pixel patch for the ith target.


### 2.6. Iterated EKF Updating

The EKF is one of the most widely used nonlinear estimators due to its similarity to the optimal linear filter, its simplicity of implementation, and its ability to provide accurate estimates in practice. We employ the iterated EKF to update the state. At each iteration stepk, the prior process state vector is computed by using (4) and then Ẑdk is calculated as a function of Xmk- by using (28). Next, the measurement innovation and the measurement Jocobian matrix are computed as Zdk-Ẑdk and Hk(2N×(13+6N)), respectively. The measurement covariance and Kalman gain can be performed as(33)Cov(Zdk)2N×2N=HkCov(Xk-)(13+6N)×(13+6N)HkT,Kk(13+6N)×2N=Cov(Xk-)HkT[Cov(Zdk)]-1.
Finally, the process state vector and its covariance are updated at each iteration state and presented as(34)Xk((13+6N)×1)=Xk-+Kk[Zdk(2N×1)-Ẑdk(2N×1)],Cov(Xk)(13+6N)×(13+6N)=Cov(Xk-)-KkHkCov(Xk-),
where Zdk is the actual measurement and detected by using image matching search. Ẑdk is the predicted measurement and computed by the sensor model. The error in the estimation is reduced by the iteration and the unknown depths of the targets converge toward the real values gradually.


### 2.7. Fast Inverse Transformation for Covariance Matrices

The inverse matrix of the measurement covariance has to be computed at each iteration step. It needs plenty of running time by using the transpose of the matrix of cofactors to invert the measurement covariance. In order to deal with the large size of the inverse matrix within the variations ofN targets efficiently, Cholesky decomposition is applied to invert the measurement covariance and reduce the running time at each iteration step. The measurement covariance is factored in the form Cov(Zdk)=LLT for a lower-left corner of the matrix L because the measurement covariance is positive semidefinite. The matrix L is not unique, and so multiple factorizations of a given matrix Cov(Zdk) are possible. A standard method for factoring positive semidefinite matrices is the Cholesky factorization. The element of L is computed by using Cholesky decomposition and addressed as follows:(35)Ljj=Ajj-∑k=1j-1Ljk2,Lij=1Ljj(Aij-∑k=1j-1LikLjk),
where Aij is the element of A=C(Zdk).A matrix equation in the formLX=b or UX=b can be solved by forward substitution for lower triangular matrix L and back substitution for upper triangular matrix U, respectively. In our proposed method, Cholesky decomposition and forward-and-back substitution are combined to invert the measurement covariance for reducing the computational loading.


## 2.1. The Origins of the Proposed Algorithm

A single camera is mounted on our system as one of the sensors. The monocular vision infers that the depth of the target is not able to be measured by only one image but estimated by the sequential images. Therefore, the single camera has to estimate the depth by observing the target repeatedly to get parallax between different captured rays from the target to the robot. The orientations of target are estimated in the world coordinate system only by one image.Yiw is a six-dimension state vector and used to describe the position of the ith target in 3D space. Its equation is addressed as(1)Yiw=[riwTθiwφiwρiw]T.riw is the location of the robot when the robot detects the ith target in the first time. Gic is defined as the position of the target with respect to the camera coordinate system and denoted by(2)Gic=Rcw((riw-rkw)+1ρiwm(θiw,φiw)).θiw and φiw are the orientations of the ith target and calculated by the pixel location of the target in only one image. The relationship between the position of the target Gic and the current robot localization rkw is presented in Figure 1.Figure 1
The robot moves fromriw to rkw. Based on the two views, the parallax is produced. The initial depth can be refined to approach the real distance and the position of the ith target is estimated.Giw is the position of the target with respect to the world frame system. 1/ρiw is defined as the distance from riw to the target and m(θiw,φiw) is its unit vector. Consequently, 1/ρiwm(θiw,φiw) is seen as the vector from riw to the target and then Giw is calculated as ((riw-rkw)+1/ρiwm(θiw,φiw)). Finally, Gic is estimated if the depth 1/ρiw is known in advance. However, the depth of the target is not able to be measured by a single image. The EKF is applied to estimate 1/ρiw, θiw, and φiw and they converge toward more correct values under the recursive iteration by using sequential image measurement information. To sum up, (2) is the basic concept and origin of our proposed algorithm.


## 2.2. Motion Modeling

We first derive the continuous-time system model that describes the time evolution in the state estimate. This model allows us to employ the sampled measurements of the wheel encoder and the compass for state propagation. The process state is described by the vector(3)Xmk=[rkwTqkwTvkwTωkcT]T,
where rkw is the position of the robot with respect to the world frame. qkw is the quaternion that represents the orientation of the world frame for the robot. The linear velocity in the world frame and the angular velocity with respect to the camera frame are denoted by vkw and ωkc, respectively. In this system, the control inputs are measured by wheel encoders and the compass which provide the linear velocity and the angular velocity, respectively. Besides, the linear velocity is used to simulate the acceleration data of the robot for describing how components of the system state vector change completely. The definition of acceleration is defined as akw=(vkw-vk-1w)/Δt. In order to have similar on-line simulation, we define akw as (vkw-vk-1w)/Δt rather than (vk+1w-vkw)/Δt. Although the former definition is not better than the later one which is closer to the real acceleration, an iterated EKF is still capable of compensating for errors when adopting (vkw-vk-1w)/Δt as the robot’s acceleration. The system model describing the time evolution of the wheel encoders and of the compass is given by(4)Xmk+1-=[rk+1wqk+1wvk+1wωk+1c]=[rkw+vkwΔt+12akwΔt2qkw×q(ωkc+Ωkc)vkw+akwΔtωkc+Ωkc].The compass is used to measureΔθk which is defined as (θk-θk-1). ωk+1c and ωkc are defined as (θk-θk-1)/Δt and (θk-1-θk-2)/Δt, respectively. If ωk+1c is assumed to be (ωkc+Ωkc), Ωkcc is derived as (θk-2θk-1+θk-2)/Δt. qk+1w is denoted by(5)qk+1w=[q1-q2-q3-q4q2q1-q4q3q3q4q1-q2q4-q3q2q1][q1′q2′q3′q4′],
where(6)q(ωkc+Ωkc)=[q1′q2′q3′q4′]=[cos⁡(Δθk2)uxsin⁡(Δθk2)uysin⁡(Δθk2)uzsin⁡(Δθk2)].u=[uxuyuz]Tis equal to ωk+1cΔt/∥ωk+1c∥Δt.The covariance is predicted and modified if considering the control covarianceCov(vkw,ωkc,akw). We assume that the process noise of the control vector is not correlated with the process state vector Xmk so that Cov(Xmk,vkw,ωkc,akw) is set to be a zero matrix. By using (A.4) in Appendix A, the predicted covariance of the system model is expressed as(7)Cov(Xmk+1-)=FXmCov(Xmk)FXmT+Q,
where(8)Q=FvCov(vkw)FvT+FωCov(ωkc)FωT+FaCov(akw)FaT.
The first item FXmCov(Xmk)FXmT represents the noise from the process state vector and Q describes the noise from the measurement of the wheel encoders and the compass. The Jacobian matrix of FXm is shown as(9)FXm=∂f∂Xm=[∂f∂rkw∂f∂qkw∂f∂vkw∂f∂ωkc]13×13.
The Jacobian matrixes of Fv and Fω are as the same as ∂f/∂vkw and ∂f/∂ωkc, respectively.


## 2.3. Adding New Target

It is remarkable about our algorithm that new targets are initialized by using only one image. The initialization includes the state initial values and the covariance assignment.


### 2.3.1. Target Initialization

Equation (1) is used to define the location of the new target and estimate the six parameters of Yiw with the EKF prediction-update loop. If the robot senses the 1st target at the state k for the first time, the new target information is added and then the process state vector is modified. The expanded process state vector is represented by the following equation:(10)Xk-=[Xmk-Y1w],
where(11)Y1w=[r1wTθ1wφ1wρ1w]6×1T.
The first time observation of the 1st target is done at the current camera location r1w. m(θ1w,φ1w) is defined as an unit vector from the location r1w to the 1st target with respect to the world frame. Figure 2 illustrates the relationship about m(θ1w,φ1w), θ1w, and φ1w. hu=[uv]T is defined as the pixel location of the 1st target in an undistorted image. gc is the location of the 1st target with respect to the camera frame. The location of the 1st target with respect to the world frame gw is addressed as(12)gw=[gxwgywgzw]=Rwcgc=Rwc[(u0-u)Dxf(v0-v)Dyf1]gzc,
where(13)Rwc=[q12+q22-q32-q422(q2q3-q1q4)2(q2q4+q1q3)2(q2q3+q1q4)q12-q22+q32-q422(q3q4-q1q2)2(q2q4-q1q3)2(q3q4+q1q2)q12-q22-q32+q42].
Then we get(14)[gxwgywgzw]gzc=Rwc[(u0-u)Dxf(v0-v)Dyf1].
Only gxw/gzc and gyw/gzc rather than gxw and gyw are computed by using hu. It is impossible to know gzc by only one image. However, it is possible to make use of gxw/gzc and gyw/gzc to calculate θ1w and φ1w which are shown as(15)[θ1wφ1w]=[tan-1⁡(gxwgzc,gzwgzc)tan-1⁡(-gywgzc,(gxwgzc)2+(gzwgzc)2)].
The unit vector m(θ1w,φ1w) is derived as a function of the orientations of the target and described as(16)m(θ1w,φ1w)=[cos⁡φ1wsin⁡θ1w-sin⁡φ1wcos⁡φ1wcos⁡θ1w].
The final parameter 1/ρ1w is not able to be measured by only one image but estimated by the sequential images with EKF prediction-update loop. As a result, it is assumed to be an initial value 1/ρ0.Figure 2
The orientations of the target with respect to the world frame can be estimated as a function of its undistorted pixel locationhu.


### 2.3.2. New Covariance Assignment

The system covariance is modified after adding a new target. By using (B.1) in Appendix B, the new covariance Cov(Xk-) and the function of Y1w are denoted by(17)Cov(Xk-)=[Cov(Xmk)B1TB1A1],Y1w=k(rkw,qkw,hd(u,v),ρ1w),
where(18)k=[r1wθ1wφ1wρ1w]=[rxryrztan-1⁡(gxwgzc,gzwgzc)tan-1(-gywgzc,⁡(gxwgzc)2+(gzwgzc)2)ρ1w].B1is not a zero matrix because the new target’s location is correlated with the estimates of the robot. According to (B.4) and (B.5) in Appendix B, B1 and A1 are derived as(19)B1=kxCov(Xmk)=[krkq06×306×3],(20)A1=krCov(r)krT+kqCov(q)kqT+a,
where (21)a=khdCov(hd)khdT+kρCov(ρ)kρT.


## 2.3.1. Target Initialization

Equation (1) is used to define the location of the new target and estimate the six parameters of Yiw with the EKF prediction-update loop. If the robot senses the 1st target at the state k for the first time, the new target information is added and then the process state vector is modified. The expanded process state vector is represented by the following equation:(10)Xk-=[Xmk-Y1w],
where(11)Y1w=[r1wTθ1wφ1wρ1w]6×1T.
The first time observation of the 1st target is done at the current camera location r1w. m(θ1w,φ1w) is defined as an unit vector from the location r1w to the 1st target with respect to the world frame. Figure 2 illustrates the relationship about m(θ1w,φ1w), θ1w, and φ1w. hu=[uv]T is defined as the pixel location of the 1st target in an undistorted image. gc is the location of the 1st target with respect to the camera frame. The location of the 1st target with respect to the world frame gw is addressed as(12)gw=[gxwgywgzw]=Rwcgc=Rwc[(u0-u)Dxf(v0-v)Dyf1]gzc,
where(13)Rwc=[q12+q22-q32-q422(q2q3-q1q4)2(q2q4+q1q3)2(q2q3+q1q4)q12-q22+q32-q422(q3q4-q1q2)2(q2q4-q1q3)2(q3q4+q1q2)q12-q22-q32+q42].
Then we get(14)[gxwgywgzw]gzc=Rwc[(u0-u)Dxf(v0-v)Dyf1].
Only gxw/gzc and gyw/gzc rather than gxw and gyw are computed by using hu. It is impossible to know gzc by only one image. However, it is possible to make use of gxw/gzc and gyw/gzc to calculate θ1w and φ1w which are shown as(15)[θ1wφ1w]=[tan-1⁡(gxwgzc,gzwgzc)tan-1⁡(-gywgzc,(gxwgzc)2+(gzwgzc)2)].
The unit vector m(θ1w,φ1w) is derived as a function of the orientations of the target and described as(16)m(θ1w,φ1w)=[cos⁡φ1wsin⁡θ1w-sin⁡φ1wcos⁡φ1wcos⁡θ1w].
The final parameter 1/ρ1w is not able to be measured by only one image but estimated by the sequential images with EKF prediction-update loop. As a result, it is assumed to be an initial value 1/ρ0.Figure 2
The orientations of the target with respect to the world frame can be estimated as a function of its undistorted pixel locationhu.


## 2.3.2. New Covariance Assignment

The system covariance is modified after adding a new target. By using (B.1) in Appendix B, the new covariance Cov(Xk-) and the function of Y1w are denoted by(17)Cov(Xk-)=[Cov(Xmk)B1TB1A1],Y1w=k(rkw,qkw,hd(u,v),ρ1w),
where(18)k=[r1wθ1wφ1wρ1w]=[rxryrztan-1⁡(gxwgzc,gzwgzc)tan-1(-gywgzc,⁡(gxwgzc)2+(gzwgzc)2)ρ1w].B1is not a zero matrix because the new target’s location is correlated with the estimates of the robot. According to (B.4) and (B.5) in Appendix B, B1 and A1 are derived as(19)B1=kxCov(Xmk)=[krkq06×306×3],(20)A1=krCov(r)krT+kqCov(q)kqT+a,
where (21)a=khdCov(hd)khdT+kρCov(ρ)kρT.


## 2.4. Measurement Modeling

The robot moves continuously and records the sequential images. This is a process of detecting and identifying the targets. The parameters of targets have been set into the process state vector and the covariance has been estimated with the recursive loop.


### 2.4.1. Sensor Model

The predicted pixel locations of targets are estimated as a function of the prior process state vector which is described in (4). According to (2), the location of the ith target in the camera frame can be defined in another way:(22)Tic=[Tx(i)cTy(i)cTz(i)c]=Rcw((riw-rkw)*ρiw+m(θiw,φiw)).
Our measurement sensor of the camera is monocular vision. There is a camera sensor model to describe how the sensor maps the variables in the process state vector into the sensor variables. By using the pinhole model [16], Tic is derived as a function of the undistorted pixel location of the ith target (ui,vi) and denoted by(23)Tic=[(u0-ui)Dxf(v0-vi)Dyf1]gz(i)cρiw.
Equation (22) and (23) are combined to derive the predicted pixel location without distortion for the ith target Zk(i)=(ui,vi). Its equation is addressed as(24)Zk(i)=[uivi]=h(Xmk-)=[u0-fDxTx(i)cTz(i)cv0-fDyTy(i)cTz(i)c].
By using image correct, the predicted pixel location of the ith target within distortion Zdk(i)=(ud(i),vd(i)) is addressed as(25)Zdk(i)=hd(Xmk-)=[(ui-u0)1+3k1rd2+5k2rd4(vi-v0)1+3k1rd2+5k2rd4]+[u0v0],
where k1 and k2 are coefficients for the radial distortion of the image.The actual image target the robot takes is the distorted pixel location rather than the undistorted one. Therefore, we have to calculateZdk(i) to get the measurement innovation for EKF updating.


### 2.4.2. Measurement Covariance Assignment

The measurement covariance is expected to describe what the likely variation of measurement is by the sensor under the current condition. The variation is infected by the variables in the process state vector and the noise which corrupts the sensor. The measurement Jacobian matrixHi is(26)Hi=[∂hd(i)∂Xmk02×6⋯∂hd(i)∂Yiw02×6⋯02×6],
where(27)∂hd(i)∂Xmk=[∂hd(i)∂rkw∂hd(i)∂qkw∂hd(i)∂vkw∂hd(i)∂ωkc]2×13,∂hd(i)∂Yiw=[∂hd(i)∂riw∂hd(i)∂θiw∂hd(i)∂φiw∂hd(i)∂ρiw]2×6.
When there are N targets observed concurrently, they are stacked in one measurement vector Zdk to form a single batch-form update equation which is shown as(28)Zdk=[Zdk(1)T⋯Zdk(N)T]T.
Similarly, the batch measurement Jacobian matrix is derived as(29)Hk=[∂h∂Xmk∂h∂Y1w⋯∂h∂YNw]=[H1T⋯HNT]T,
where(30)Hk=[∂h1∂Xmk2×13∂h1∂Y1w2×602×602×6…02×6∂h2∂Xmk2×1302×6∂h2∂Y2w2×602×6…02×6∂h3∂Xmk2×1302×602×6∂h3∂Y3w2×6…02×6⋮⋮⋮⋮⋱⋮∂hN∂Xmk2×1302×602×602×6…∂hN∂YNw2×6].


## 2.4.1. Sensor Model

The predicted pixel locations of targets are estimated as a function of the prior process state vector which is described in (4). According to (2), the location of the ith target in the camera frame can be defined in another way:(22)Tic=[Tx(i)cTy(i)cTz(i)c]=Rcw((riw-rkw)*ρiw+m(θiw,φiw)).
Our measurement sensor of the camera is monocular vision. There is a camera sensor model to describe how the sensor maps the variables in the process state vector into the sensor variables. By using the pinhole model [16], Tic is derived as a function of the undistorted pixel location of the ith target (ui,vi) and denoted by(23)Tic=[(u0-ui)Dxf(v0-vi)Dyf1]gz(i)cρiw.
Equation (22) and (23) are combined to derive the predicted pixel location without distortion for the ith target Zk(i)=(ui,vi). Its equation is addressed as(24)Zk(i)=[uivi]=h(Xmk-)=[u0-fDxTx(i)cTz(i)cv0-fDyTy(i)cTz(i)c].
By using image correct, the predicted pixel location of the ith target within distortion Zdk(i)=(ud(i),vd(i)) is addressed as(25)Zdk(i)=hd(Xmk-)=[(ui-u0)1+3k1rd2+5k2rd4(vi-v0)1+3k1rd2+5k2rd4]+[u0v0],
where k1 and k2 are coefficients for the radial distortion of the image.The actual image target the robot takes is the distorted pixel location rather than the undistorted one. Therefore, we have to calculateZdk(i) to get the measurement innovation for EKF updating.


## 2.4.2. Measurement Covariance Assignment

The measurement covariance is expected to describe what the likely variation of measurement is by the sensor under the current condition. The variation is infected by the variables in the process state vector and the noise which corrupts the sensor. The measurement Jacobian matrixHi is(26)Hi=[∂hd(i)∂Xmk02×6⋯∂hd(i)∂Yiw02×6⋯02×6],
where(27)∂hd(i)∂Xmk=[∂hd(i)∂rkw∂hd(i)∂qkw∂hd(i)∂vkw∂hd(i)∂ωkc]2×13,∂hd(i)∂Yiw=[∂hd(i)∂riw∂hd(i)∂θiw∂hd(i)∂φiw∂hd(i)∂ρiw]2×6.
When there are N targets observed concurrently, they are stacked in one measurement vector Zdk to form a single batch-form update equation which is shown as(28)Zdk=[Zdk(1)T⋯Zdk(N)T]T.
Similarly, the batch measurement Jacobian matrix is derived as(29)Hk=[∂h∂Xmk∂h∂Y1w⋯∂h∂YNw]=[H1T⋯HNT]T,
where(30)Hk=[∂h1∂Xmk2×13∂h1∂Y1w2×602×602×6…02×6∂h2∂Xmk2×1302×6∂h2∂Y2w2×602×6…02×6∂h3∂Xmk2×1302×602×6∂h3∂Y3w2×6…02×6⋮⋮⋮⋮⋱⋮∂hN∂Xmk2×1302×602×602×6…∂hN∂YNw2×6].


## 2.5. Image Matching by Using Cross-Correlation

The image patch information of the targets whose variables are set in the process state vector has been stored in the data-base when the robot senses them in the first time. The predicted pixel locations of the targets within distortion are estimated by using (25). Next important issue is how to search for the actual pixel locations of the targets in a 320 × 240 image. Image matching is a fundamental and vital aspect of many problems in computer vision including motion tracking and object recognition. The image features are invariant to rotation, image scaling, change in illumination, and 3D camera viewpoint. It is still a popular research topic regarding how to allow a single candidate image to be correctly matched with high probability. Based on the estimation of measurement covariance, the small area where the target lies with high probability is able to be predicted. The measurement covariance of the ith target is defined as(31)Cov(Zdk(i))2×2=[σuu(i)2σuv(i)2σvu(i)2σvv(i)2].The template match technique is used to search the actual image pixel location of theith target in the small search area whose width and length are kσuu(i) and kσvv(i), respectively. The coefficient k is a constant and defines how large the search area is. The larger k is, the more is the search time. There are two advantages regarding the search area which is estimated by the measurement covariance. One advantage is that a lot of time is saved because the image pixel location of the ith target is detected in a small search area rather than in a 320 × 240 image. The other one is that the successful search rate increases dramatically because the search area allows the image pixel location to be correctly matched with high probability. As a result, it is not necessary to use a complicated object recognition algorithm such as Scale Invariant Feature Transform (SIFT) [17] for image matching in our system. Cross-correlation search is applied to be our template match algorithm and the computing loading is lower due to its simplification. This approach uses cross-correlation of image to search a suitable image patch. ID is defined as the template image patch for the ith target and stored in the database. It is defined as a candidate image patch in the search area whose width is kσuu(i) and length is kσvv(i). The cross-correlation value of It with ID is given by(32)1M2-1∑uv(ID(u,v)-I̅D)(It(u,v)-I̅t)σIDσIt.M2 is the number of pixels in an image patch and σID and σIt are the stand deviations of ID and ID, respectively. I̅D and I̅t are the average values of ID and It, respectively. The maximum value of cross-correlation of It with ID is the best template matching and it is seen as the matching target pixel patch for the ith target.


## 2.6. Iterated EKF Updating

The EKF is one of the most widely used nonlinear estimators due to its similarity to the optimal linear filter, its simplicity of implementation, and its ability to provide accurate estimates in practice. We employ the iterated EKF to update the state. At each iteration stepk, the prior process state vector is computed by using (4) and then Ẑdk is calculated as a function of Xmk- by using (28). Next, the measurement innovation and the measurement Jocobian matrix are computed as Zdk-Ẑdk and Hk(2N×(13+6N)), respectively. The measurement covariance and Kalman gain can be performed as(33)Cov(Zdk)2N×2N=HkCov(Xk-)(13+6N)×(13+6N)HkT,Kk(13+6N)×2N=Cov(Xk-)HkT[Cov(Zdk)]-1.
Finally, the process state vector and its covariance are updated at each iteration state and presented as(34)Xk((13+6N)×1)=Xk-+Kk[Zdk(2N×1)-Ẑdk(2N×1)],Cov(Xk)(13+6N)×(13+6N)=Cov(Xk-)-KkHkCov(Xk-),
where Zdk is the actual measurement and detected by using image matching search. Ẑdk is the predicted measurement and computed by the sensor model. The error in the estimation is reduced by the iteration and the unknown depths of the targets converge toward the real values gradually.


## 2.7. Fast Inverse Transformation for Covariance Matrices

The inverse matrix of the measurement covariance has to be computed at each iteration step. It needs plenty of running time by using the transpose of the matrix of cofactors to invert the measurement covariance. In order to deal with the large size of the inverse matrix within the variations ofN targets efficiently, Cholesky decomposition is applied to invert the measurement covariance and reduce the running time at each iteration step. The measurement covariance is factored in the form Cov(Zdk)=LLT for a lower-left corner of the matrix L because the measurement covariance is positive semidefinite. The matrix L is not unique, and so multiple factorizations of a given matrix Cov(Zdk) are possible. A standard method for factoring positive semidefinite matrices is the Cholesky factorization. The element of L is computed by using Cholesky decomposition and addressed as follows:(35)Ljj=Ajj-∑k=1j-1Ljk2,Lij=1Ljj(Aij-∑k=1j-1LikLjk),
where Aij is the element of A=C(Zdk).A matrix equation in the formLX=b or UX=b can be solved by forward substitution for lower triangular matrix L and back substitution for upper triangular matrix U, respectively. In our proposed method, Cholesky decomposition and forward-and-back substitution are combined to invert the measurement covariance for reducing the computational loading.


## 3. The Analysis of the Algorithm

In this section, there are two key points about the proposed algorithm presented as follows. The first key point is that the details of algorithm will be analyzed to prove that it is not necessary to get the depth information in our algorithm even though it is designed to track the 3D localization of the targets. The other key point is introduced to explain why the simple template match technique, cross-correlation search, is applied.


### 3.1. Depth Information Analysis in the Proposed Algorithm

The depth information should be known while the robot localizes the targets in 3D space by using monocular vision. However, the proposed algorithm does not need the depth information to localize targets. As presented in (14), it is not necessary to know the depth information to calculate θ1w and φ1w. Similarly, it seems that the depth information should be known to compute kq in (19). kq is derived as(36)kq=[∂riw∂qkwT∂θiw∂qkwT∂φiw∂qkwT∂ρiw∂qkwT]6×4T,
where(37)∂θiw∂qkw=∂θiw∂gw*∂gw∂qkw.
The depth information gzw has to be known for calculating ∂θiw/∂gw according to the following equation:(38)∂θiw∂gw=[gzw(gxw)2+(gzw)20-gxw(gxw)2+(gzw)2].
However, ∂θiw/∂qkw still can be computed without knowing gzw because of ∂gw/∂qkw. The details are proved as follows:(39)∂θiw∂qkw=[gzw/gzc(gxw/gzc)2+(gzw/gzc)20-gxw/gzc(gxw/gzc)2+(gzw/gzc)2]×1gzc∂gw∂qkw,
where(40)1gzc∂gw∂qkw=[∂Rwc∂q1gcgzc∂Rwc∂q2gcgzc∂Rwc∂q3gcgzc∂Rwc∂q4gcgzc].gxw/gzc, gyw/gzc, and gzw/gzc are computed by (14). Equation (39) can be calculated without knowing the depth information gzc due to the following equation:(41)gcgzc=[(u0-u)Dxf(v0-v)Dyf1].In the same way, the depth information should be known to computekhd in equation (19). khd is derived as(42)khd=[∂riw∂hdT∂θiw∂hdT∂φiw∂hdT∂ρiw∂hdT]6×2T,
where(43)∂θiw∂hd=∂θiw∂gw∂gw∂gc∂gc∂hu∂hu∂hd.∂gw/∂gc=Rwc and ∂hu/∂hd can be computed without gzc but ∂θiw/∂gw cannot be calculated without gzw according to (39). However, ∂θiw/∂hd still can be estimated without knowing gzc and gzw if ∂θiw/∂gw and ∂gc/∂hu are combined. The details are proved by the following equations:(44)∂θiw∂gw∂gc∂hu=[gzw/gzc(gxw/gzc)2+(gzw/gzc)20-gxw/gzc(gxw/gzc)2+(gzw/gzc)2]×1gzc*∂gc∂hu,
where(45)∂gc∂hu=[-Dxf00-Dyf00]gzc.
Therefore, ∂θiw/∂hd is computed without gzc and gzw by using (44) and applying (14) to calculate gxw/gzc,gyw/gzc and gzw/gzc.Based on the results of calculating∂θiw/∂qkw, ∂θiw/∂hd and (14), it infers that we have solved a problem that the depth information, gzc and gzw, is not able to be measured only by one image. This is a very important characteristic of the proposed algorithm.


### 3.2. Analysis Regarding Using Cross-Correlation Search

Generally speaking, cross-correlation search is though as a simple template match algorithm and it is not as robust as SIFT. However,ID is still detected correctly in the search area by using cross-correlation search because the small area is estimated by an iterated EKF and it includes the actual pixel location of the target with high probability. As shown in Figure 3, the predicted pixel locations of the targets are estimated by using the sensor model and denoted by the green crosses. The pixel locations of the red crosses are the corresponding target pixel locations and detected by applying cross-correlation search. Based on the testing result that the red crosses are closer to the actual target pixel locations, it has proved that it is feasible for the proposed algorithm to apply cross-correlation search as its template match algorithm.Figure 3
Detect the actual target pixel location by using cross-correlation search.


## 3.1. Depth Information Analysis in the Proposed Algorithm

The depth information should be known while the robot localizes the targets in 3D space by using monocular vision. However, the proposed algorithm does not need the depth information to localize targets. As presented in (14), it is not necessary to know the depth information to calculate θ1w and φ1w. Similarly, it seems that the depth information should be known to compute kq in (19). kq is derived as(36)kq=[∂riw∂qkwT∂θiw∂qkwT∂φiw∂qkwT∂ρiw∂qkwT]6×4T,
where(37)∂θiw∂qkw=∂θiw∂gw*∂gw∂qkw.
The depth information gzw has to be known for calculating ∂θiw/∂gw according to the following equation:(38)∂θiw∂gw=[gzw(gxw)2+(gzw)20-gxw(gxw)2+(gzw)2].
However, ∂θiw/∂qkw still can be computed without knowing gzw because of ∂gw/∂qkw. The details are proved as follows:(39)∂θiw∂qkw=[gzw/gzc(gxw/gzc)2+(gzw/gzc)20-gxw/gzc(gxw/gzc)2+(gzw/gzc)2]×1gzc∂gw∂qkw,
where(40)1gzc∂gw∂qkw=[∂Rwc∂q1gcgzc∂Rwc∂q2gcgzc∂Rwc∂q3gcgzc∂Rwc∂q4gcgzc].gxw/gzc, gyw/gzc, and gzw/gzc are computed by (14). Equation (39) can be calculated without knowing the depth information gzc due to the following equation:(41)gcgzc=[(u0-u)Dxf(v0-v)Dyf1].In the same way, the depth information should be known to computekhd in equation (19). khd is derived as(42)khd=[∂riw∂hdT∂θiw∂hdT∂φiw∂hdT∂ρiw∂hdT]6×2T,
where(43)∂θiw∂hd=∂θiw∂gw∂gw∂gc∂gc∂hu∂hu∂hd.∂gw/∂gc=Rwc and ∂hu/∂hd can be computed without gzc but ∂θiw/∂gw cannot be calculated without gzw according to (39). However, ∂θiw/∂hd still can be estimated without knowing gzc and gzw if ∂θiw/∂gw and ∂gc/∂hu are combined. The details are proved by the following equations:(44)∂θiw∂gw∂gc∂hu=[gzw/gzc(gxw/gzc)2+(gzw/gzc)20-gxw/gzc(gxw/gzc)2+(gzw/gzc)2]×1gzc*∂gc∂hu,
where(45)∂gc∂hu=[-Dxf00-Dyf00]gzc.
Therefore, ∂θiw/∂hd is computed without gzc and gzw by using (44) and applying (14) to calculate gxw/gzc,gyw/gzc and gzw/gzc.Based on the results of calculating∂θiw/∂qkw, ∂θiw/∂hd and (14), it infers that we have solved a problem that the depth information, gzc and gzw, is not able to be measured only by one image. This is a very important characteristic of the proposed algorithm.


## 3.2. Analysis Regarding Using Cross-Correlation Search

Generally speaking, cross-correlation search is though as a simple template match algorithm and it is not as robust as SIFT. However,ID is still detected correctly in the search area by using cross-correlation search because the small area is estimated by an iterated EKF and it includes the actual pixel location of the target with high probability. As shown in Figure 3, the predicted pixel locations of the targets are estimated by using the sensor model and denoted by the green crosses. The pixel locations of the red crosses are the corresponding target pixel locations and detected by applying cross-correlation search. Based on the testing result that the red crosses are closer to the actual target pixel locations, it has proved that it is feasible for the proposed algorithm to apply cross-correlation search as its template match algorithm.Figure 3
Detect the actual target pixel location by using cross-correlation search.


## 4. Experimental Results

In order to validate the proposed algorithm for localizing targets when the ground truth is available we have performed a number of experiments. The algorithm is implemented by C++ and performed by PC with 2.0 GHz microprocessor. The monocular sensor is a single camera with wide angle lens because we hope that more targets can be observed in one image and tracking rate can be higher. The camera’s field of view is 170° with a focal length of 1.7 mm. The image measurements received at a rate of 15 Hz are distorted with noiseσ=20 pixel. The addressed experimental results are tested under the high speed robot motion because the average velocity of each case is higher than 20 cm/sec and the maximum velocity of all cases is 69.11 cm/sec. For the duration of experiments, the initial distance between the camera and the targets ranges from 1.68 m to 5.76 m. The unknown depth of the target is estimated by sequential images with EKF and six cases (3.0 m, 3.5 m, 4.0 m, 4.5 m, 5.0 m, and 5.5 m) are assumed to be default depths for each experiment case. All of the sensors mounted on our system are shown in Figure 4.Figure 4
The encoder shown in (a) is used to measure the linear velocity and simulate the acceleration for the robot. The electrical compass shown in (b) is applied to measure the angular velocity of the robot. As shown in (c), the camera with wide angle lens is the additional measurement sensor. The three sensors are combined to estimate the localization of targets.There are some measurement errors caused by the camera distortion when using the camera with wide angle lens. Before validating the proposed algorithm, we performed an experiment to estimate the distorted noise by making use of the artificial landmarks. We chose the corners of the artificial landmarks as targets. The undistorted pixel locations of the corners are estimated by the pinhole model and then the image correction is applied to compute their predicted pixel locations with distortion. Owing to using a camera with wide angle lens, there is an error between the predicted pixel location with distortion and its actual pixel location which is detected by the cross-correlation search. In the proposed algorithm, the predicted pixel location without distortion is estimated in terms of the prior process state vector. The distorted error is produced if transforming the undistorted pixel locations of targets to the distorted pixel locations. Therefore, the distorted error should be taken into consideration very carefully. Based on the experimental result, the distorted noise is assumed to be 20 pixels.


### 4.1. Results of Target Localization

For demonstrating the performance and practical applications to the proposed MVLT, the experiments for a mobile robot go through the doors by tracking the feature targets located upon the door and localize robot location simultaneously.


#### 4.1.1. Target Localization Experiments

Once a robot has to pass through doors, one should recognize the accurate doors position. The experiment shown in Figure5 is designed to verify accuracy of MVLT for the door as tracked targets. Figure 6 shows the real environmental features as targets whose positions require to be estimated for a task that the robot passing through the door. Since the developed system does not require an initial setting at the first iteration, it provides a practical usage under a dynamical motion if target is detected instead of training processes of the environment features for robot localization. The two different moving paths with varied linear velocities and orientations are examined as shown in Figure 7. The initial true depth for paths A and B between robot and the doors is about 3.5 meter, path A is a straight forward path, and path B is a left way of robot to move forward to the doors. The experimental result is summarized in Table 1, and the averaged error of proposed approach is about 10 cm.Table 1
Error comparison of target tracking.
AlgorithmItemEstimated target position (m)Track OKTarget distance error (m)XYZMVTLTarget 1 in Path A0.76000.14230.6528OK0.1369Target 2 in Path A0.7800−0.03740.6909OK0.1303Target 1 in Path B0.81000.09750.6853OK0.0839Target 2 in Path B0.7600−0.11350.5819OK0.1523MVTL Average Error0.1049Target 1 in Path A0.78000.04600.8234OK0.2042Target 2 in Path A0.8370−0.02181.0201OK0.3723MonoSLAM [18]Target 1 in Path B1.11580.08961.2330Failed0.6160Target 2 in Path B1.2281−0.11611.4326Failed0.8435MonoSLAM [18] Average Error0.5090Figure 5
The experiments are designed to localize the natural landmarks as targets with varied linear and angular velocity patterns in different moving paths including the straight and curved paths.Figure 6
The predicted pixel locations of the targets are denoted by the green crosses and estimated by using the sensor model. The pixel locations of the red crosses are the corresponding target pixel locations and detected by applying cross-correlation search.Figure 7
Ground truth of experiments.According to the experimental result shown in Figures8 and 9, it infers that the camera is not able to provide the precise measurement information to make MonoSLAM [18] correcting the prior process state variables under a high-speed movement when the control input is not provided. It also shows that the image-distorted noise σ=20 pixel is too large for MonoSLAM [18] model. Owing to combining hybrid sensor information, the proposed MVTL algorithm is capable of localizing targets with image distorted noise σ=20 pixel and its average error is lower than 0.11 m. It has proved that MVTL algorithm is robust to track targets under a higher-speed movement with larger measurement noise.Figure 8
The comparison of the target 1 depth errors (meter) between MVTL and MonoSLAM [18] in Path B.Figure 9
The comparison of the target 2 depth errors (meter) between MVTL and MonoSLAM [18] in Path B.


#### 4.1.2. Acceleration Approximation-Based Error Comparison

We also performed an experiment about the comparison between encoders and IMU. We choose an encoder as our sensor instead of IMU in indoor environment. The accelerationa(k)w is approximated as (v(k)w-v(k-1)w)/Δt by encoders in order to have similar on-line simulation data. It will increase additional acceleration noise if we use prevelocity v(k)w rather than v(k+1)w to simulate the acceleration of the robot at state k. However, an iterated EKF is robust enough to compensate for errors from this low-cost sensor and the definition of acceleration. The error of acceleration from a simulated IMU is lower than 0.001 m/s2. Based on the result shown in Table 2, the errors of localization by using encoders are still accepted even though we use prevelocity v(k)w to simulate acceleration at iterated state k. It has proved that errors are able to be reduced gradually with EKF recursive loop.Table 2
Comparison of localization errors between the encoder and IMU.
Robot motionMaximum Velocity (m/sec)Average Velocity (m/sec)IMU Error (m)Encoder Error (m)Low Velocity0.3630.2540.0520.089High Velocity0.5820.4460.1420.144


#### 4.1.3. Performance of Depth Initial Conditions

In terms of the experimental result shown in Figure10, we could conclude that the ability of localizing targets depends on the parallax from different views rather than the distance between the robot and the target. It is a very crucial viewpoint to analyze the stability of the system in terms of means and deviations. Not only do we analyze one the target localization but also we understand the details about multiple target localization. Refering to the above experiments, MVTL algorithm localizes targets with a higher-speed motion and six kinds of default depth values converge to the similar value with little errors.Figure 10
The unknown depth of the target is estimated by sequential images with EKF and six cases (3.0 m, 3.5 m, 4.0 m, 4.5 m, 5.0 m, and 5.5 m) are assumed to be default depths for each experiment case.


### 4.2. Experiments of Fast Inverse Matrix Transformation

Finally, we performed an experiment to verify Cholesky decomposition and forward-and-back substitution. In this experiment, the totally testing time is 4.2 sec with 15 Hz image rate. At first, the transpose of the matrix of cofactors is used to invert the measurement covariance matrix whose size ranges from 2 × 2 to 8 × 8. The experimental results shown in Table3 present that the running time is long if the system inverts a 6 × 6 or a more dimension matrix. However, the running time is reduced dramatically if the system inverts an 8 × 8 matrix by using Cholesky decomposition and forward-and-back substitution. The test result shows that the system is capable of being real-time even though localizing four targets concurrently.Table 3
Comparison of execution time.
Target number1234Cholesky decomposition (sec)0.8771.1821.5481.92Matrix of cofactors (sec)0.8711.2167.07516.742


## 4.1. Results of Target Localization

For demonstrating the performance and practical applications to the proposed MVLT, the experiments for a mobile robot go through the doors by tracking the feature targets located upon the door and localize robot location simultaneously.


### 4.1.1. Target Localization Experiments

Once a robot has to pass through doors, one should recognize the accurate doors position. The experiment shown in Figure5 is designed to verify accuracy of MVLT for the door as tracked targets. Figure 6 shows the real environmental features as targets whose positions require to be estimated for a task that the robot passing through the door. Since the developed system does not require an initial setting at the first iteration, it provides a practical usage under a dynamical motion if target is detected instead of training processes of the environment features for robot localization. The two different moving paths with varied linear velocities and orientations are examined as shown in Figure 7. The initial true depth for paths A and B between robot and the doors is about 3.5 meter, path A is a straight forward path, and path B is a left way of robot to move forward to the doors. The experimental result is summarized in Table 1, and the averaged error of proposed approach is about 10 cm.Table 1
Error comparison of target tracking.
AlgorithmItemEstimated target position (m)Track OKTarget distance error (m)XYZMVTLTarget 1 in Path A0.76000.14230.6528OK0.1369Target 2 in Path A0.7800−0.03740.6909OK0.1303Target 1 in Path B0.81000.09750.6853OK0.0839Target 2 in Path B0.7600−0.11350.5819OK0.1523MVTL Average Error0.1049Target 1 in Path A0.78000.04600.8234OK0.2042Target 2 in Path A0.8370−0.02181.0201OK0.3723MonoSLAM [18]Target 1 in Path B1.11580.08961.2330Failed0.6160Target 2 in Path B1.2281−0.11611.4326Failed0.8435MonoSLAM [18] Average Error0.5090Figure 5
The experiments are designed to localize the natural landmarks as targets with varied linear and angular velocity patterns in different moving paths including the straight and curved paths.Figure 6
The predicted pixel locations of the targets are denoted by the green crosses and estimated by using the sensor model. The pixel locations of the red crosses are the corresponding target pixel locations and detected by applying cross-correlation search.Figure 7
Ground truth of experiments.According to the experimental result shown in Figures8 and 9, it infers that the camera is not able to provide the precise measurement information to make MonoSLAM [18] correcting the prior process state variables under a high-speed movement when the control input is not provided. It also shows that the image-distorted noise σ=20 pixel is too large for MonoSLAM [18] model. Owing to combining hybrid sensor information, the proposed MVTL algorithm is capable of localizing targets with image distorted noise σ=20 pixel and its average error is lower than 0.11 m. It has proved that MVTL algorithm is robust to track targets under a higher-speed movement with larger measurement noise.Figure 8
The comparison of the target 1 depth errors (meter) between MVTL and MonoSLAM [18] in Path B.Figure 9
The comparison of the target 2 depth errors (meter) between MVTL and MonoSLAM [18] in Path B.


### 4.1.2. Acceleration Approximation-Based Error Comparison

We also performed an experiment about the comparison between encoders and IMU. We choose an encoder as our sensor instead of IMU in indoor environment. The accelerationa(k)w is approximated as (v(k)w-v(k-1)w)/Δt by encoders in order to have similar on-line simulation data. It will increase additional acceleration noise if we use prevelocity v(k)w rather than v(k+1)w to simulate the acceleration of the robot at state k. However, an iterated EKF is robust enough to compensate for errors from this low-cost sensor and the definition of acceleration. The error of acceleration from a simulated IMU is lower than 0.001 m/s2. Based on the result shown in Table 2, the errors of localization by using encoders are still accepted even though we use prevelocity v(k)w to simulate acceleration at iterated state k. It has proved that errors are able to be reduced gradually with EKF recursive loop.Table 2
Comparison of localization errors between the encoder and IMU.
Robot motionMaximum Velocity (m/sec)Average Velocity (m/sec)IMU Error (m)Encoder Error (m)Low Velocity0.3630.2540.0520.089High Velocity0.5820.4460.1420.144


### 4.1.3. Performance of Depth Initial Conditions

In terms of the experimental result shown in Figure10, we could conclude that the ability of localizing targets depends on the parallax from different views rather than the distance between the robot and the target. It is a very crucial viewpoint to analyze the stability of the system in terms of means and deviations. Not only do we analyze one the target localization but also we understand the details about multiple target localization. Refering to the above experiments, MVTL algorithm localizes targets with a higher-speed motion and six kinds of default depth values converge to the similar value with little errors.Figure 10
The unknown depth of the target is estimated by sequential images with EKF and six cases (3.0 m, 3.5 m, 4.0 m, 4.5 m, 5.0 m, and 5.5 m) are assumed to be default depths for each experiment case.


## 4.1.1. Target Localization Experiments

Once a robot has to pass through doors, one should recognize the accurate doors position. The experiment shown in Figure5 is designed to verify accuracy of MVLT for the door as tracked targets. Figure 6 shows the real environmental features as targets whose positions require to be estimated for a task that the robot passing through the door. Since the developed system does not require an initial setting at the first iteration, it provides a practical usage under a dynamical motion if target is detected instead of training processes of the environment features for robot localization. The two different moving paths with varied linear velocities and orientations are examined as shown in Figure 7. The initial true depth for paths A and B between robot and the doors is about 3.5 meter, path A is a straight forward path, and path B is a left way of robot to move forward to the doors. The experimental result is summarized in Table 1, and the averaged error of proposed approach is about 10 cm.Table 1
Error comparison of target tracking.
AlgorithmItemEstimated target position (m)Track OKTarget distance error (m)XYZMVTLTarget 1 in Path A0.76000.14230.6528OK0.1369Target 2 in Path A0.7800−0.03740.6909OK0.1303Target 1 in Path B0.81000.09750.6853OK0.0839Target 2 in Path B0.7600−0.11350.5819OK0.1523MVTL Average Error0.1049Target 1 in Path A0.78000.04600.8234OK0.2042Target 2 in Path A0.8370−0.02181.0201OK0.3723MonoSLAM [18]Target 1 in Path B1.11580.08961.2330Failed0.6160Target 2 in Path B1.2281−0.11611.4326Failed0.8435MonoSLAM [18] Average Error0.5090Figure 5
The experiments are designed to localize the natural landmarks as targets with varied linear and angular velocity patterns in different moving paths including the straight and curved paths.Figure 6
The predicted pixel locations of the targets are denoted by the green crosses and estimated by using the sensor model. The pixel locations of the red crosses are the corresponding target pixel locations and detected by applying cross-correlation search.Figure 7
Ground truth of experiments.According to the experimental result shown in Figures8 and 9, it infers that the camera is not able to provide the precise measurement information to make MonoSLAM [18] correcting the prior process state variables under a high-speed movement when the control input is not provided. It also shows that the image-distorted noise σ=20 pixel is too large for MonoSLAM [18] model. Owing to combining hybrid sensor information, the proposed MVTL algorithm is capable of localizing targets with image distorted noise σ=20 pixel and its average error is lower than 0.11 m. It has proved that MVTL algorithm is robust to track targets under a higher-speed movement with larger measurement noise.Figure 8
The comparison of the target 1 depth errors (meter) between MVTL and MonoSLAM [18] in Path B.Figure 9
The comparison of the target 2 depth errors (meter) between MVTL and MonoSLAM [18] in Path B.


## 4.1.2. Acceleration Approximation-Based Error Comparison

We also performed an experiment about the comparison between encoders and IMU. We choose an encoder as our sensor instead of IMU in indoor environment. The accelerationa(k)w is approximated as (v(k)w-v(k-1)w)/Δt by encoders in order to have similar on-line simulation data. It will increase additional acceleration noise if we use prevelocity v(k)w rather than v(k+1)w to simulate the acceleration of the robot at state k. However, an iterated EKF is robust enough to compensate for errors from this low-cost sensor and the definition of acceleration. The error of acceleration from a simulated IMU is lower than 0.001 m/s2. Based on the result shown in Table 2, the errors of localization by using encoders are still accepted even though we use prevelocity v(k)w to simulate acceleration at iterated state k. It has proved that errors are able to be reduced gradually with EKF recursive loop.Table 2
Comparison of localization errors between the encoder and IMU.
Robot motionMaximum Velocity (m/sec)Average Velocity (m/sec)IMU Error (m)Encoder Error (m)Low Velocity0.3630.2540.0520.089High Velocity0.5820.4460.1420.144


## 4.1.3. Performance of Depth Initial Conditions

In terms of the experimental result shown in Figure10, we could conclude that the ability of localizing targets depends on the parallax from different views rather than the distance between the robot and the target. It is a very crucial viewpoint to analyze the stability of the system in terms of means and deviations. Not only do we analyze one the target localization but also we understand the details about multiple target localization. Refering to the above experiments, MVTL algorithm localizes targets with a higher-speed motion and six kinds of default depth values converge to the similar value with little errors.Figure 10
The unknown depth of the target is estimated by sequential images with EKF and six cases (3.0 m, 3.5 m, 4.0 m, 4.5 m, 5.0 m, and 5.5 m) are assumed to be default depths for each experiment case.


## 4.2. Experiments of Fast Inverse Matrix Transformation

Finally, we performed an experiment to verify Cholesky decomposition and forward-and-back substitution. In this experiment, the totally testing time is 4.2 sec with 15 Hz image rate. At first, the transpose of the matrix of cofactors is used to invert the measurement covariance matrix whose size ranges from 2 × 2 to 8 × 8. The experimental results shown in Table3 present that the running time is long if the system inverts a 6 × 6 or a more dimension matrix. However, the running time is reduced dramatically if the system inverts an 8 × 8 matrix by using Cholesky decomposition and forward-and-back substitution. The test result shows that the system is capable of being real-time even though localizing four targets concurrently.Table 3
Comparison of execution time.
Target number1234Cholesky decomposition (sec)0.8771.1821.5481.92Matrix of cofactors (sec)0.8711.2167.07516.742


## 5. Conclusion and Future Work

### 5.1. Conclusion

Based on the different experiments we performed, it has proved that the proposed algorithm is able to localize targets with “cm” level accuracy under a higher-speed movement. Besides, we have validated that it is practical to use odometry sensors to track targets as well. Not only does the system start the recursive procedure without the initial setting but also the robot can move rapidly to localize targets. The EKF-based algorithm is really practical and robust to reduce errors from sensors even though the low-cost sensors are used in our system. The efficiency of the proposed algorithm is impressive by using Cholesky decomposition and some computational tricks. In terms of the experimental result shown in Figure10, we could conclude that the ability of localizing targets depends on the parallax from different views rather than the distance between the robot and the target. Consequently, the proposed algorithm has a high potential extension to surveillance and monitoring for UAVs with aerial odometry sensors. In the same way, it is able to be used widely for robot tasks as well.


### 5.2. Future Work

The targets are assumed to be stationary landmarks in the proposed algorithm. It will be an interesting and challenging research if the algorithm is modified to track a moving target. This type of the technique is necessary and going to be widely used in many applications. For instance, UGV has to know the intended motions of other moving objects in order to plan its navigating path for the next state. Therefore, it will be a good future work to add a new approach into the proposed algorithm to track moving targets. Besides, there is another important future work for our system. This future work is to find some ways or algorithms to improve accuracy of the measurement data by the low-cost sensor because it will improve the localization errors in terms of more accurate measurement data.


## 5.1. Conclusion

Based on the different experiments we performed, it has proved that the proposed algorithm is able to localize targets with “cm” level accuracy under a higher-speed movement. Besides, we have validated that it is practical to use odometry sensors to track targets as well. Not only does the system start the recursive procedure without the initial setting but also the robot can move rapidly to localize targets. The EKF-based algorithm is really practical and robust to reduce errors from sensors even though the low-cost sensors are used in our system. The efficiency of the proposed algorithm is impressive by using Cholesky decomposition and some computational tricks. In terms of the experimental result shown in Figure10, we could conclude that the ability of localizing targets depends on the parallax from different views rather than the distance between the robot and the target. Consequently, the proposed algorithm has a high potential extension to surveillance and monitoring for UAVs with aerial odometry sensors. In the same way, it is able to be used widely for robot tasks as well.


## 5.2. Future Work

The targets are assumed to be stationary landmarks in the proposed algorithm. It will be an interesting and challenging research if the algorithm is modified to track a moving target. This type of the technique is necessary and going to be widely used in many applications. For instance, UGV has to know the intended motions of other moving objects in order to plan its navigating path for the next state. Therefore, it will be a good future work to add a new approach into the proposed algorithm to track moving targets. Besides, there is another important future work for our system. This future work is to find some ways or algorithms to improve accuracy of the measurement data by the low-cost sensor because it will improve the localization errors in terms of more accurate measurement data.



---

*Source: 548042-2012-01-11.xml*