When tracking an object’s position, it is typically done from the ego vehicle’s perspective. However, the ego’s motion makes tracking the object somewhat difficult. The basic idea is to perform the tracking in the world coordinate system, then transform the results into the ego-car’s coordinate system. In this post, we will discuss how to concisely incorporate the ego’s motion into the object tracking process.
Continuous Form
Coordinate Definitions
The target’s movement in the world coordinate: $\mathbf{o}(t)$; ego-car movement in the world coordinate: $\mathbf{g}(t)$; ego-car’s heading angle: $\theta(t)$; observed target’s coordinate relative to the ego-car: $\mathbf{x}(t)$.
Continuous Motion in the World Coordinate
$$\mathbf{E}(t)\mathbf{x}(t) = \boldsymbol{o}(t)- \mathbf{g}(t) \tag{1} $$where
$$ \mathbf{E}(t) = \begin{bmatrix} \cos \theta(t) & -\sin\theta(t) \\ \sin \theta(t) & \cos\theta(t) \end{bmatrix} $$is the basis and it also represents the rotation of the ego car, $\mathbf{x}(t) = [x_x(t), x_y(t)]^\top$ is the observed relative position at time $t$ under the basis $\mathbf{E}(t)$, $\mathbf{o}(t)=[o_x(t), o_y(t)]^\top$ and $\mathbf{g}(t) = [g_x(t), g_y(t)]^\top$ are the object’s and ego-car’s motion in the world coordinate, respectively. Taking first and second derivatives of the equation (1) with respect to time, we have
$$ \partial \mathbf{E} \mathbf{x} + \mathbf{E} \partial \mathbf{x} = \partial (\mathbf{o} -\mathbf{g}) \tag{2} $$and
$$ \partial^2\mathbf{E} \mathbf{x} + 2\partial\mathbf{E}\partial\mathbf{x} + \mathbf{E}\partial^2\mathbf{x} = \partial^2(\mathbf{o} - \mathbf{g}) \tag{3} $$According to equation (1), the observed relative position satisfies
$$ \mathbf{x}(t) = \mathbf{E}^{-1}(t)\left(\mathbf{o}(t) - \mathbf{g}(t)\right) \tag{4} $$Thus, the observed object’svelocity satisfies
$$ \begin{aligned} \partial \mathbf{x} = \partial \mathbf{E}^{-1} \,(\mathbf{o} - \mathbf{g}) + \mathbf{E}^{-1}\,(\partial \mathbf{o} - \partial \mathbf{g}) \end{aligned} \tag{5} $$and the observed object’s acceleration satisfies
$$ \begin{aligned} \partial^2 \mathbf{x} =& \partial^2 \mathbf{E}^{-1}(\mathbf{o} - \mathbf{g}) + \partial \mathbf{E}^{-1}(\partial \mathbf{o} - \partial\mathbf{g}) \\ &+ \partial \mathbf{E}^{-1}(\partial \mathbf{o} - \partial \mathbf{g}) + \mathbf{E}^{-1}(\partial^2 \mathbf{o} - \partial^2\mathbf{g}) \\ =&\partial^2 \mathbf{E}^{-1}(\mathbf{o} - \mathbf{g}) + 2\partial \mathbf{E}^{-1}(\partial \mathbf{o} - \partial\mathbf{g}) + \mathbf{E}^{-1}(\partial^2 \mathbf{o} - \partial^2\mathbf{g}) \end{aligned} \tag{6} $$where
$$ \mathbf{E} = \begin{bmatrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{bmatrix}, $$$$ \mathbf{E}^{-1} = \mathbf{E}^\top = \begin{bmatrix} \cos\theta & \sin\theta \\ -\sin\theta & \cos\theta \end{bmatrix}, $$$$ \begin{aligned} \partial\mathbf{E} &= \partial\theta \begin{bmatrix} -\sin\theta & -\cos\theta \\ \cos\theta & -\sin\theta \end{bmatrix}\\ &=\omega \mathbf{E} \begin{bmatrix} 0 & -1 \\ 1 & 0 \end{bmatrix}\\ &\triangleq \omega \mathbf{E}\mathbf{C}\\ &= \omega \mathbf{C}\mathbf{E}\\ &= \mathbf{E}[\omega]_\times \end{aligned}, $$$$ \begin{aligned} \partial^2\mathbf{E} &= \partial \left(\omega \mathbf{E}\mathbf{C}\right)\\ &= \dot{\omega} \mathbf{E}\mathbf{C} + \omega \partial \mathbf{E}\mathbf{C}\\ &= \dot{\omega} \mathbf{E}\mathbf{C} - \omega^2 \mathbf{E}\\ &= \mathbf{E}[\dot{\omega}\mathbf{C}-\omega^2\mathbf{I}]\\ &= \mathbf{E}\left([\dot{\omega}]_\times - \omega^2\mathbf{I}\right)\\ \end{aligned}, $$$$ \begin{aligned} \partial\mathbf{E}^{-1} &= \partial\theta \cdot \begin{bmatrix} -\sin\theta & \cos\theta\\ -\cos\theta & -\sin\theta \end{bmatrix}\\ &= \omega \mathbf{E}^{-1}\mathbf{C}^{-1}\\ &= \omega \mathbf{C}^{-1}\mathbf{E}^{-1}\\ &= (\partial \mathbf{E})^\top \end{aligned}, $$$$ \begin{aligned} \partial^2\mathbf{E}^{-1} &= \partial \left(\omega \mathbf{C}^{-1}\mathbf{E}^{-1}\right)\\ &= (\dot{\omega}\mathbf{C}^{-1} - \omega^2\mathbf{I}) \mathbf{E}^{-1}\\ &= \mathbf{E}^{-1}(\dot{\omega}\mathbf{C}^{-1} - \omega^2\mathbf{I})\\ &= \left(\partial^2 \mathbf{E}\right)^\top \end{aligned}, $$and $\omega = \dot{\theta}$ is the angular velocity, $\mathbf{C} = \begin{bmatrix}0 & -1\\1 & 0\end{bmatrix}$, $\mathbf{C}^\top\mathbf{C} = \mathbf{I}$, $\mathbf{C}\mathbf{C} = - \mathbf{I}$.
With these notations in hand, let us re-examine Eq. (2) and Eq. (3) further. They are closely related to the rigid-body motion [1]. In Eq. (2),
$$ \begin{aligned} \partial\mathbf{E} \mathbf{x} &=\omega \mathbf{E}\mathbf{C}\mathbf{x} &= \mathbf{E} [\omega ]_\times\mathbf{x} \end{aligned}, $$where $[\omega]_\times$ is the skew-symmetric matrix of $\omega$, defined as
$$ [\omega]_\times = \begin{bmatrix} 0 & -\omega \\ \omega & 0 \end{bmatrix}. $$Recall attitude-kinematics equation [1]:
$$ \dot{\mathbf{R}} = \mathbf{R}[\omega]_\times. $$It is a special case of the formula in the 2D space.
If we embed the plane into a 3D space, then the rotation $\omega$ can be represented as vector, $[0, 0, \omega]^\top$, and a point, $[x, y]$ in 3D is $[x, y, 0]^\top$. Then the cross product of $\omega$ and $\mathbf{x}$ is
$$ \begin{bmatrix} 0\\ 0\\ \omega \end{bmatrix} \times \begin{bmatrix} x \\ y \\ 0 \end{bmatrix}= \omega \begin{bmatrix} -y\\ x\\ 0 \end{bmatrix}. $$Its first two elements are indeed $[\omega]_\times\mathbf{x}$. It represents the tangential velocity. Then the velocity Eq. (2) can be rewritten as
$$ \mathbf{E}(\mathbf{v}_r + \mathbf{v}_t) = \mathbf{v}_o - \mathbf{v}_g, $$where $\mathbf{v}_r = \partial \mathbf{x}/ \partial t$ is relative velocity, $\mathbf{v}_t$ is the tangential velocity. If we want to connect the velocity between local and world, we need to compensate the tangential velocity. Further, if the ego-car’s velocity is given in its own coordinate system, i.e., $\mathbf{E}\mathbf{v}_e = \mathbf{v}_g$, we have
$$ \mathbf{v}_o = \mathbf{E}(\mathbf{v}_r + \mathbf{v}_t + \mathbf{v}_e). \tag{7} $$This equation gives us clear physical meaning of the velocities.
For the acceleration Eq. (3), we have
$$ \mathbf{E}\big[([\dot{\omega}]_\times - \omega^2\mathbf{I})\mathbf{x} + 2[\omega]_\times \mathbf{v}_t + \mathbf{a}_x \big] = \mathbf{a}_o - \mathbf{a}_g. $$Similarly, if we give the ego-car’s acceleration in its own coordinate system, i.e., $\mathbf{E}\mathbf{a}_e = \mathbf{a}_g$, we have
$$ \mathbf{a}_o = \mathbf{E}\big[([\dot{\omega}]_\times - \omega^2\mathbf{I})\mathbf{x} + 2[\omega]_\times \mathbf{v}_r + \mathbf{a}_e \big]. $$Form of State Basis
In the following, we will derive the general form for the basis of states, $\mathbf{B}$.
We can write Eq. (1)-(6) in a matrix form, like
$$ \mathbf{B}\mathbf{x} = \mathbf{o} - \mathbf{g}, $$or
$$ \mathbf{x} = \mathbf{B}^{-1}(\mathbf{o} - \mathbf{g}). $$According to Eq. (1), (2) and (3), for a constant velocity model we have
$$ \mathbf{B} = \begin{bmatrix} \mathbf{E} & \mathbf{0} \\ \partial \mathbf{E} & \mathbf{E} \end{bmatrix} $$and for the constant acceleration model we have
$$ \mathbf{B} = \begin{bmatrix} \mathbf{E} & \mathbf{0} & \mathbf{0} \\ \partial \mathbf{E} & \mathbf{E} & \mathbf{0} \\ \partial^2 \mathbf{E} & 2\partial\mathbf{E} & \mathbf{E} \end{bmatrix}. $$According to Eq. (4), (5), and (6), for a constant velocity model, we have
$$ \mathbf{B}^{-1} = \begin{bmatrix} \mathbf{E}^{-1} & \mathbf{0} \\ \partial\mathbf{E}^{-1} & \mathbf{E}^{-1} \end{bmatrix} $$and for a constant acceleration model we have
$$ \mathbf{B}^{-1} = \begin{bmatrix} \mathbf{E}^{-1} & \mathbf{0} & \mathbf{0} \\ \partial\mathbf{E}^{-1} & \mathbf{E}^{-1} & \mathbf{0} \\ \partial^2\mathbf{E}^{-1} & 2\partial\mathbf{E}^{-1} & \mathbf{E}^{-1} \end{bmatrix}. $$Then, for constant velocity model, we have
$$ \mathbf{B} = \text{diag}(\mathbf{E}, \mathbf{E}) \cdot \begin{bmatrix} \mathbf{I} & \mathbf{0} \\ \omega\mathbf{C} & \mathbf{I} \end{bmatrix} = \text{diag}(\mathbf{E}, \mathbf{E})\mathbf{W}, $$$$ \mathbf{B}^{-1} = \begin{bmatrix} \mathbf{I} & \mathbf{0}\\ \omega\mathbf{C}^{-1} & \mathbf{I} \end{bmatrix} \cdot \text{diag}(\mathbf{E}^{-1}, \mathbf{E}^{-1}) =\mathbf{W}^{-1}\cdot\text{diag}(\mathbf{E}^{-1}, \mathbf{E}^{-1}). $$For state that contains position, velocity, and acceleration, we have
$$ \begin{aligned} \mathbf{B} &= \text{diag}(\mathbf{E}, \mathbf{E}, \mathbf{E}) \cdot \begin{bmatrix} \mathbf{I} & \mathbf{0} & \mathbf{0}\\ \omega\mathbf{C} & \mathbf{I} & \mathbf{0} \\ \dot{\omega}\mathbf{C}-\omega^2\mathbf{I} & 2\omega\mathbf{C} & \mathbf{I} \end{bmatrix} \\ &= \text{diag}(\mathbf{E}, \mathbf{E}, \mathbf{E})\mathbf{W} \end{aligned} $$and
$$ \begin{aligned} \mathbf{B}^{-1} &= \begin{bmatrix} \mathbf{I} & \mathbf{0} & \mathbf{0}\\ \omega\mathbf{C}^{-1} & \mathbf{I} &\mathbf{0}\\ \dot{\omega}\mathbf{C}^{-1}-\omega^2\mathbf{I} & 2\omega\mathbf{C}^{-1} & \mathbf{I} \end{bmatrix} \cdot \text{diag}(\mathbf{E}^{-1}, \mathbf{E}^{-1},\mathbf{E}^{-1}) \\ &= \mathbf{W}^{-1}\text{diag}(\mathbf{E}^{-1}, \mathbf{E}^{-1},\mathbf{E}^{-1}) \end{aligned} $$Continuous Motion Model
The motion ordinary differential equation (ODE) is
$$\dot{\mathbf{o}}(t) = \mathbf{A}\mathbf{o}(t) + \mathbf{n}(t)$$where if it is a constant velocity model, we have $\mathbf{o}(t) = [s(t), \dot{s}(t)]^\top $,
$$ \mathbf{A} = \begin{bmatrix} 0 & 1\\ 0 & 0 \end{bmatrix} $$$\mathbf{n}(t) = [0, n]^\top$, and if it is a constant acceleration model we have $\mathbf{o}(t) = [s(t), \dot{s}(t), \ddot{s}(t)]^\top$,
$$ \mathbf{A} = \begin{bmatrix} 0 & 1 & 0\\ 0 & 0 & 1\\ 0 & 0 & 0 \end{bmatrix} $$$\mathbf{n}(t) = [0, 0, n]^\top$.
Discretized Form
Please refere to the Appendix of the pose tracking post for how to discretize the continuous motion model.
We would like to derive a recursive formula for the relative states, $\mathbf{x}$, where $\mathbf{x}$ can contain the relative position, velocity, and acceleration. For the objective and ego, $\mathbf{o}$ and $\mathbf{g}$ can also contain the position, velocity, and acceleration. The state transition and observation functions are
$$ \begin{aligned} \mathbf{B}_{k-1}\mathbf{x}_{k-1} &= \mathbf{o}_{k-1} - \mathbf{g}_{k-1} & (8.1)\\ \mathbf{B}_{k} \mathbf{x}_{k} &= \mathbf{o}_{k} - \mathbf{g}_{k} & (8.2)\\ \mathbf{o}_{k} &= \mathbf{F}_{k}\circ\mathbf{o}_{k-1} + \mathbf{n}_k & (8.3)\\ \mathbf{y}_{k} &= \mathbf{H}_{k}\mathbf{x}_{k} + \mathbf{w}_{k} & (8.4) \end{aligned} $$where $\mathbf{y}_{k}$ and $\mathbf{w}_{k}$ are the observation and observation noise, respectively. “$\circ$” denotes the function composition. $\mathbf{B}$ is the basis matrix as discussed in previous section.
Substitue equation (8.1) and (8.2) into equation (8.3), we have a recurvise relationship between relative states in the ego-car’s coordinates at time $k-1$ and $k$ is:
$$ \mathbf{B}_{k}\mathbf{x}_{k} + \mathbf{g}_{k}= \mathbf{F}_k\circ [\mathbf{B}_{k-1}\mathbf{x}_{k-1}+ \mathbf{g}_{k-1}]+ \mathbf{n}_k $$Then the predicted states in the ego-car’s coordinate at time $k$:
$$ \mathbf{x}_k = \mathbf{B}_k^{-1}[\mathbf{F}_k\circ(\mathbf{B}_{k-1}\mathbf{x}_{k-1}+\mathbf{g}_{k-1}) - \mathbf{g}_{k} + \mathbf{n}_k] \tag{9} $$Ego-pose Information
In a regular system, the information of the ego-car includes: the translation, $\mathbf{t}_k$ and the coordinate rotation matrix $\mathbf{R}_k$ from $k-1$ to $k$, or ego velocity, $\mathbf{v}_k$, the angular velocity, $\omega_k$. Represent the ego-car’s information in the world coordinate we have
$$ \mathbf{E}_k\mathbf{t}_k = \mathbf{g}_{k-1} - \mathbf{g}_{k} ,\quad \mathbf{E}_k\mathbf{v}_k ,\quad \mathbf{R}_k = \mathbf{E}_k^{-1}\mathbf{E}_{k-1} $$Ego Motion Compensation
According to Eq. (9) we have:
$$\begin{aligned} \mathbf{x}_{k} &= \mathbf{B}_{k}^{-1} [\mathbf{F}_k\circ(\mathbf{B}_{k-1}\mathbf{x}_{k-1} +\mathbf{g}_{k-1}) - \mathbf{g}_{k} + \mathbf{n}_k] \\ &= \mathbf{B}^{-1}_{k} \left( \mathbf{F}_k\circ(\mathbf{B}_{k-1}\mathbf{x}_{k-1}) + \mathbf{F}_{k}\circ\mathbf{g}_{k-1} - \mathbf{g}_{k}+ \mathbf{n}_k \right) \end{aligned}$$And assume the ego motion has the same motion pattern as for the object, then we have
$$ \begin{aligned} \mathbf{F}_k\circ\mathbf{g}_{k-1} - \mathbf{g}_k = \mathbf{n}_{g_k} \end{aligned} $$where $\mathbf{n}_{g_k}$ is the process noise for the ego-car at time $k$.
Object Part in the Motion Model
The part that involves the object in the motion model is $\mathbf{B}_k^{-1}\mathbf{F}_k\circ\mathbf{B}_{k-1}\mathbf{x}_{k-1}$. Substitute the expression of $\mathbf{B}_k^{-1}$ we have
$$ \begin{aligned} \mathbf{B}_k^{-1}\mathbf{F}_k\circ(\mathbf{B}_{k-1}\mathbf{x}_{k-1}) &\overset{(a)}{=} \mathbf{W}_k^{-1}\cdot\text{diag}(\mathbf{E}_{k}^{-1}, \mathbf{E}_{k}^{-1}, \cdots) \cdot \mathbf{F}_k \cdot\text{diag}(\mathbf{E}_{k-1}, \mathbf{E}_{k-1}, \cdots)\mathbf{W}_{k-1}\mathbf{x}_{k-1} \\ &\overset{(b)}{=} \mathbf{W}_k^{-1}\text{diag}(\mathbf{R}_k, \mathbf{R}_k, \cdots)\mathbf{F}_k\mathbf{W}_{k-1}\mathbf{x}_{k-1} \\ &\overset{(c)}{=} \mathbf{W}_k^{-1}\mathbf{F}_k\text{diag}(\mathbf{R}_k, \mathbf{R}_k, \cdots)\mathbf{W}_{k-1}\mathbf{x}_{k-1}\\ &\overset{(d)}{=} \bar{\mathbf{R}}_k\mathbf{W}_k^{-1}\mathbf{F}_k\mathbf{W}_{k-1}\mathbf{x}_{k-1}\\ &\triangleq\bar{\mathbf{R}}_k\hat{\mathbf{F}}_k\mathbf{x}_{k-1} \end{aligned} $$where $\mathbf{R}_k = \mathbf{E}_k^{-1}\mathbf{E}_{k-1} = \mathbf{E}(\theta_{k-1} - \theta_k)$, $\bar{\mathbf{R}}_k = \text{diag}(\mathbf{R}_k, \cdots)$, (a) established by substitute $\mathbf{F}_k\circ$ by its matrix formular, $\mathbf{F}_k\cdot$, (b) and (c) substatiates due to the assumption that $\bar{\mathbf{R}}\mathbf{F} = \mathbf{F}\bar{\mathbf{R}}$, which is true for a constant velocity or constant acceleration model, (d) is true as a result of $\bar{\mathbf{R}}\mathbf{W}^{-1}=\mathbf{W}^{-1} \bar{\mathbf{R}}$. It indicates that the order of following motion model and rotate coordinate does not matter. We can formulate the motion model only depending on the relative rotation because we can exchange the motion and the coordinate.
Conclusion
The complete Kalman filter with ego-motion compensation is as follows:
Do prediction with pseudo motion matrix,
Subtract the ego rotation
Follow motion model
Add ego rotation
Rotate
Compensate the ego-motion
Do correction as in traditional Kalman filter.
Applications
Static Object Position Tracking
For a static object, its position does not change. So, the motion matrix is
$\mathbf{F} = \mathbf{I}$, $\mathbf{B} = \mathbf{E}$, and $\mathbf{W}=\mathbf{I}$.
We have $\hat{\mathbf{F}}_k = \mathbf{R}_k$. So that
$$ \mathbf{x}_k =\mathbf{R}_k \mathbf{x}_{k-1} + \mathbf{E}_k^{-1}\mathbf{n}_{g_k} + \mathbf{n}_k $$where $\mathbf{n}_{g_k} = \mathbf{g}_{k-1} - \mathbf{g}_k=\mathbf{E}_k\mathbf{t}_k$. Thus, we get
$$ \mathbf{x}_k =\mathbf{R}_k\mathbf{x}_{k-1} + \mathbf{t}_k + \mathbf{n}_k $$It means, in this case, we need to compensate for the relative translation and rotation due to the ego-motion.
Constant Velocity Object Position Tracking
The Pseudo Motion Matrix
$$ \begin{aligned} \hat{\mathbf{F}}_k &=\bar{\mathbf{W}}_k^{-1}\mathbf{F}_k\mathbf{W}_{k-1} \\ &= \mathbf{F}_k + \begin{bmatrix} \Delta{t}\omega_{k-1}\mathbf{C} & \mathbf{0} \\ \Delta{t}\omega_k\omega_{k-1}\mathbf{I} + (\omega_{k-1}-\omega_k)\mathbf{C} & -\Delta{t}\omega_k\mathbf{C} \end{bmatrix} \\ &\overset{(a)}{=}\mathbf{F}_k + \Delta{\theta} \begin{bmatrix} \mathbf{I} & \mathbf{0} \\ -\omega_k\mathbf{C} & -\mathbf{I} \end{bmatrix} \begin{bmatrix} \mathbf{C} & \mathbf{0} \\ \mathbf{0} & \mathbf{C} \end{bmatrix} \end{aligned} $$where (a) is true with assumption $\omega_k = \omega_{k-1}$. The second term which involves the rotated angle is usually ignored by others.
The complete form of $\hat{\mathbf{F}}_k$ is
$$\mathbf{F}_k + \left[\begin{matrix}0 & - \Delta{t} \omega_{k-1} & 0 & 0\\\Delta{t} \omega_{k-1} & 0 & 0 & 0\\\Delta{t} \omega_{k-1} \omega_{k} & - \omega_{k-1} + \omega_{k} & 0 & \Delta{t}\omega_{k}\\\omega_{k-1} - \omega_{k} & \Delta{t} \omega_{k-1} \omega_{k} & - \Delta{t} \omega_{k} & 0\end{matrix}\right]$$The Ego-motion Part
Since the motion matrix $\mathbf{F}_k$ follows the constant velocity model:
- If we assume ego motion is at constant velocity, $$ \mathbf{F}_k\mathbf{g}_{k-1}- \mathbf{g}_k =\mathbf{0} $$
The results are interesting, which are counter intuitive at first glance. We do not need to account for ego-motion.
- If we assume ego motion is at constant acceleration,
where $a$ is the acceleration of the ego-car.
Constant Acceleration Object Position Tracking
The Pseudo Motion Matrix
With the assumption that $\dot{\omega}_k = 0$, $\dot{\omega}_{k-1}=0$, and $\omega_k = \omega_{k-1}$:
$$\begin{aligned} \hat{\mathbf{F}}_k &=\bar{\mathbf{W}}_k^{-1}\mathbf{F}_k\mathbf{W}_{k-1} \\ &= \mathbf{F}_k + \Delta{\theta} \begin{bmatrix} -0.5\Delta{\theta} & -1 & 0 & -\Delta{t} & 0 & 0 \\ 1 & -0.5\Delta{\theta} & \Delta{t}& 0&0 &0\\ 0 & -0.5\Delta{\theta}\omega & \Delta{\theta} & -1& 0& 0.5\Delta{t}\\ 0.5\Delta{\theta}\omega & 0 & 1 & \Delta{\theta} & -0.5\Delta{t} & 0\\ 0.5\Delta{\theta}\omega^2 & -\omega^2 & 3\omega & \Delta{\theta}\omega & -0.5\Delta{\theta}& 2 \\ \omega^2 & 0.5\Delta{\theta}\omega^2 & -\Delta{\theta}\omega & 3\omega & -2 & -0.5\Delta{\theta} \end{bmatrix} \\ &= \mathbf{F}_k + \Delta{\theta}\begin{bmatrix} -0.5\Delta{\theta}\mathbf{I} + \mathbf{C} & \Delta{t}\mathbf{C} & \mathbf{0} \\ 0.5\Delta{\theta}\mathbf{C} & \Delta{\theta}\mathbf{I} + \mathbf{C} & -0.5\Delta{t}\mathbf{C} \\ -\omega^2\mathbf{C}+0.5\Delta{\theta}\omega^2\mathbf{I} & 3\omega\mathbf{I}-\Delta{\theta}\omega\mathbf{C} & -0.5\Delta{\theta}\mathbf{I}-2\mathbf{C} \end{bmatrix} \\ &= \mathbf{F}_k + \Delta{\theta} \begin{bmatrix} 0.5\Delta{\theta}\mathbf{C} + \mathbf{I} & \Delta{t}\mathbf{I} & \mathbf{0} \\ 0.5\Delta{\theta}\mathbf{I} & -\Delta{\theta}\mathbf{C} + \mathbf{I} & -0.5\Delta{t}\mathbf{I} \\ -\omega^2\mathbf{I}-0.5\Delta{\theta}\omega^2\mathbf{C} & -3\omega\mathbf{C} - \Delta{\theta}\omega\mathbf{I} & 0.5\Delta{\theta}\mathbf{C} - 2\mathbf{I} \end{bmatrix} \cdot \text{diag}(\mathbf{C}, \mathbf{C}, \mathbf{C}) \end{aligned}$$Summary
In ego-motion compensation, if the motion model of the object and ego-car is different, we need to compensate for ego-motion. Otherwise, we do not need to. We also need the basis rotation angle, $\theta$, rotation rate, $\omega$, and rotation acceleration, $\dot{\omega}$ to get the full state transition equation.
Pitfalls
One may try to estimate the state of the object in the world coordinate, which means
$$\begin{aligned} \mathbf{E}_k \mathbf{y}_k + \mathbf{g}_k &= \mathbf{H}_k \mathbf{x}_k +\mathbf{w}_k \\ \mathbf{x}_k &= \mathbf{F}_k \mathbf{x}_{k-1} + \mathbf{n}_k \end{aligned}$$One first converts the current observed coordinate to the world coordinate by doing $\mathbf{E}_k\mathbf{y}_k + \mathbf{g}_k$, then doing the ordinary Kalman updates. Finally, converts the estimate hidden state by $\mathbf{E}_k^\top[ \hat{\mathbf{x}}_{k} - \hat{\mathbf{g}}_k]$, where $\hat{\mathbf{x}}_k$ is the updated states in the world coordinate and $\hat{\mathbf{g}}_k = [ \mathbf{g}_k, \mathbf{0}]^\top$. However, this method is wrong. Because we can not directly transform the other states (velocity, acceleration, etc.) by simply duplicate operations as in the position transform. As shown in Eq. (1) and Eq. (2), the transformation of velocity and acceleration not only depends on the rotation matrix, it also depends on other attributes of the ego-poses.
References
[1] R. M. Murray, Z. Li, and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, 1st ed. CRC Press, 2017. doi: 10.1201/9781315136370.