control input($u_t$)이 이전 state($x_{t-1}$)을 현재 state($x_t$)로 변화시킬 사후확률(posterior probability), 스테이트 전이 확률을 의미한다.

자동차의 바퀴에 장착된 wheel encoder의 센서 데이터를 이용한 모델, 모션 명령을 실행한 후에만 센서 값을 얻을 수 있기 때문에 추정을 위해 적용한다.

• $u=<\delta_{rot1}, \delta_{rot2}, \delta{trans}>$
  

자동차의 무게중심 또는 후륜중앙에 부착된 imu와 같은 관성 센서를 이용한 모델

• $u=(v, w)^T$, where $v$ is translation velocity and $w$ is rotational velocity

현재 state에서의 센서 데이터의 확률을 의미하며 Model for laser scanners의 경우 다음과 같다.

• $z_t=(z_t^1, \dots, z_t^k)$: Scan $z$ consists of $K$ measurements.
• $p(z_t|x_t)=\Pi_{i=1}^kp(z_{t}^i|x_t)$: Individual measurements are independent given the robot position

3차원 공간에서 SO(3) 군은 원점을 중심으로 회전하는 연산에 대한 군을 말한다. 이는 로보틱스에서 강체(rigid body)의 움직임 중 회전을 표현하는데 주로 사용된다. rigid body motion이란 물체의 형태가 변화하지 않아서 각도, 상대적인 회전량, 거리 등이 보존되는 움직임을 의미한다.

• SO(3)군은 일반적으로 회전행렬(rotation matrix)를 사용하여 표기한다.
• Quaternion을 사용하여 SO(3)를 표기하는 방법 또한 널리 사용된다.

• 회전행렬의 직교 성질: $R^TR=I=RR^T$
• 회전행렬의 역행렬은 전지행렬과 동일: $R^{-1}=R^T$
• 강체의 움직임: $det(R)=1$
• 합성은 행렬의 곱셈 연산: $R_1\cdot R_2=R_3\in SO(3)$

3D rotation matrices can be obtainded from these three using matrix multiplication.
Generally, ZYX rotation is used. yaw, pitch, and roll angles are $\alpha, \beta$ and $\gamma$ respectively.

1. 기준 좌표계(Global Coordinate)을 X축(Roll)을 기준으로 $\gamma$ 만큼 회전
2. 기준 좌표계(Global Coordinate)을 Y축(Pitch)을 기준으로 $\beta$ 만큼 회전
3. 기준 좌표계(Global Coordinate)을 Z축(Yaw)을 기준으로 $\alpha$ 만큼 회전

• Rotation matrix has has inverse matrix and transposed matrix which indicates roration in the opposite direction.
• Sets that satisfy the properties of rotation matrix belongs to SO(3) (Special Orthogonal Group).
• SO(3) groups can only express rotations. The 4X4 matrix must be considered to represent Translation, It should be extended to homogeneous coordinate.
• In this 4X4 matrix, the determinant = +1 of R(rotation), and the group that forms the affine rigid motion is called SE(3).

$$q=\begin{bmatrix}q_w&q_x&q_y&q_z\end{bmatrix}^T$$

$Q = q_w+q_xi+q_yj+q_zk \Leftrightarrow Q=q_w+\boldsymbol{q_v}$

where $\boldsymbol{q_v}$ is vector $(x, y, z)$. Therefore, a quaternion can be posed as a sum scalar($q_w$) + vector($\boldsymbol{q_v}$).

• $q^{*}=q_w-\boldsymbol{q_v}$
• For unit quaternions, $||q||=1$, and therefore $q^{-1}(inverse) = q^{*}(conjugate)$.

• If the axis of rotation is the x-axis

$q_w = cos(\alpha/2)$, $q_x = sin(\alpha/2) * 1$, $q_y = sin(\alpha/2) * 0$, $q_z = sin(\alpha/2) * 0$

• If the axis of rotation is the y-axis

$q_w = cos(\alpha/2)$, $q_x = sin(\alpha/2) * 0$, $q_y = sin(\alpha/2) * 1$, $q_z = sin(\alpha/2) * 0$

• If the axis of rotation is the z-axis

$q_w = cos(\alpha/2)$, $q_x = sin(\alpha/2) * 0$, $q_y = sin(\alpha/2) * 0$, $q_z = sin(\alpha/2) * 1$

where $\alpha$ is rotation angle.

#define _USE_MATH_DEFINES
#include <cmath>

struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};

// this implementation assumes normalized quaternion
// converts to Euler angles in 3-2-1 sequence
EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = std::atan2(sinr_cosp, cosr_cosp);

    // pitch (y-axis rotation)
    double sinp = std::sqrt(1 + 2 * (q.w * q.y - q.x * q.z));
    double cosp = std::sqrt(1 - 2 * (q.w * q.y - q.x * q.z));
    angles.pitch = 2 * std::atan2(sinp, cosp) - M_PI / 2;

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}

• n차원의 좌표를 n+1개의 좌표로 나타내는 것
• 좌측은 Homogeneous coordinate 우측은 Cartesian(Euclidean) coordinate을 의미한다.

$$X= \begin{bmatrix} u\\ v\\ w \end{bmatrix} = w\begin{bmatrix} x\\ y\\ 1 \end{bmatrix} = \begin{bmatrix} x\\ y\\ 1 \end{bmatrix} \Leftrightarrow X = \begin{bmatrix} x\\ y \end{bmatrix}$$

• 3차원에서의 회전행렬(Rotation matrix)와 평행 이동행렬(translation matrix) 다음과 같이 정의된다.

$$R^{3D}(w, \phi, \kappa)=R_z^{3D}(\kappa)R_y^{3D}(\phi)R_x^{3D}(w)$$

$$t= \begin{bmatrix} t_x\\ t_y\\ t_z\end{bmatrix}$$

• 따라서 3차원에서의 최종적으로 물체의 회전과 이동을 한번에 표현하는 homogeneous transformation matrix는 다음과 같다.

$$M= \lambda \begin{bmatrix} R & t\\ 0^T & 1 \end{bmatrix}$$