## 3 Humanoid Robot State Estimation Based on a Particle Filter  
In this section, we first introduce a humanoid robot imitation system based on particle filter with neither dimension reduction of the state space nor motion model construction. Then, we highlight the drawbacks of this system, motivating the use of GPDM. We used the NAO humanoid robot manufactured by Softbank Robotics for the experiments of state estimation in the imitation system. A Microsoft Kinect V2 sensor was used for acquiring human motion data. The joint angles of the humanoid robot were estimated by a particle filter, as the robot imitated human motion. Our experimental procedure is close to that by [PERSON] et al. [8] and [PERSON] and [PERSON] [19]. The humanoid robot imitates human motions using the particle filter system, as shown in Figure 1. Human skeleton joint data are obtained from the Kinect sensor, and the robot joint angles are estimated using the particle filter. The state space of the particle filter system has 25 dimensions, corresponding to the 25 actuated joints of the robot. The human skeleton joints detected by the Kinect sensor are used for calculating the particle weights during the estimation of the humanoid robot joint angles. The joints of the human skeleton model are shown in Figure 5. The elements in vector \({}^{h}\mathbf{link}=\left[{}^{h}link_{1},{}^{h}link_{2},\cdots,{}^{h}link_{11}\right]\) are shown as green lines and correspond to the model links, which were used to calculate the similarity between the human and robot poses. The similarity is used for calculating the particle weights. Link \({}^{h}link_{1}\) joins HEAD and SPINE_SHOULDER, and the other links, similarly join other skeleton joints to resemble the human body structure. The links of the humanoid robot are defined similarly, and the corresponding vector is denoted as \({}^{T}\mathbf{link}\). Pose similarity \(sim\) between the human and robot is calculated by a cosine similarity measurement:  
\[sim=\frac{1}{N_{link}}\sum_{i=1}^{N_{link}}\frac{{}^{h}\mathbf{link}_{i}\cdot^{r} \mathbf{link}_{i}}{\left\|{}^{h}\mathbf{link}_{i}\right\|\left\|{}^{r}\mathbf{link}_{i} \right\|}, \tag{1}\]  
where \(N_{link}\) is the number of links. Then, weight \(w\) of a particle is calculated by the following likelihood function:  
\[w=\exp(-\frac{1}{2\sigma^{2}}(1-sim)^{2}). \tag{2}\]  
The particles are defined in a 25D joint angle space, and the particle set is denoted as \(P_{t}=[\mathbf{p}_{t}^{1},\mathbf{p}_{t}^{2},\cdots,\mathbf{p}_{t}^{\Omega}]\), where \(Q\) is the number of particles. Each particle is given by \(\mathbf{p}_{t}^{q}=[\theta_{t}^{1},\theta_{t}^{2},\cdots,\theta_{t}^{M}]\), where \(M\) is number of the humanoid robot joints. The estimation process using particle filter is detailed in Algorithm 1. Figure 5: Skeleton joints and links of human body model. A motion model is also important for particle state tracking in the particle filter. However, it is difficult to determine an analytical function for the 25D space of the robot joint angles. Instead, a simple motion model can be defined as  
\[\mathbf{p}_{t}=\mathbf{p}_{t-1}+\mathbf{\varepsilon},\ \ \ \ \ \mathbf{\varepsilon}\sim \mathcal{N}(\mathbf{0},\mathbf{\sigma}^{2})\Delta t, \tag{3}\]  
where \(\Delta t\) is the sampling time and \(\mathcal{N}(\mathbf{0},\mathbf{\sigma}^{2})\) denotes Gaussian noise with zero mean and variance \(\mathbf{\sigma}^{2}\). Below, we replace the motion model with one based on GPDM. The joint angles of the humanoid robot can be estimated as the weighted mean of the particles:  
\[\mathbf{p}_{t}=\frac{\sum_{q=1}^{Q}\mathbf{p}_{t}^{q}w_{t}^{q}}{\sum_{q=1}^{Q}w_{t}^{q}}, \tag{4}\]  
where \(w_{t}^{q}\) is the weight of particle \(q\) at time \(t\).