Table 2 Model terms of the mobile robot examples
From: Robust error estimation based on factor-graph models for non-line-of-sight localization
Linear motion | Nonlinear motion |
|---|---|
\(x_t=f(x_{t-1},u_t)+v_t\) | \(x_t=f(x_{t-1},u_t)+v_t\) |
\(\quad =Fx_{t-1}+Bu_t +v_t\) | \(\quad =x_{t-1}+\left[ \begin{array}{c} \lambda _{t-1}T \cos \theta _{t-1}\\ \lambda _{t-1}T \sin \theta _{t-1}\\ 0\\ 0 \end{array}\right] +Bu_t+v_t\) |
\(v_t\sim N(0,Q)\) | \(v_t\sim N(0,Q)\) |
\(x_t=(x_t,\dot{x}_t, y_t,\dot{y}_t)\) | \(x_t=(x_t, y_t,\theta _t,\lambda _t)\) |
\(z_t=g(x_t)+w_t\) | \(z_t=g(x_t)+w_t\) |
\(w_t\sim N(0,R)\) | \(w_t\sim N(0,R)\) |
\(F=\partial f/\partial x_{t-1}\) | \(F_{t-1}=\partial f/\partial x_{t-1}\) |
\(\quad =\left[ \begin{array}{cccc} 1&{}T&{}0&{}0\\ 0&{}1&{}0&{}0\\ 0&{}0&{}1&{}T\\ 0&{}0&{}0&{}1 \end{array} \right]\) | \(\quad =\left[ \begin{array}{cccc} 1&{}0&{}-\lambda _{t-1}T\sin \theta _{t-1}&{}T\cos \theta _{t-1}\\ 0&{}1&{}\lambda _{t-1}T\cos \theta _{t-1}&{}T\sin \theta _{t-1}\\ 0&{}0&{}1&{}0\\ 0&{}0&{}0&{}1\\ \end{array} \right]\) |
\(B=\begin{bmatrix} 0 &{} 0\\ 1 &{} 0\\ 0 &{} 0\\ 0 &{} 1 \end{bmatrix}\), Â Â Â \(G=\left[ \begin{array}{cccc} 1&{}0&{}0&{}0\\ 0&{}0&{}1&{}0\\ \end{array} \right]\) | \(B=\left[ \begin{array}{cc} 0&{}0\\ 0&{}0\\ 1&{}0\\ 0&{}1 \end{array} \right]\), Â Â Â \(G=\left[ \begin{array}{cccc} 1&{}0&{}0&{}0\\ 0&{}1&{}0&{}0\\ \end{array} \right]\) |
\(Q=\left[ \begin{array}{cccc} \sigma ^2_{Q1}&{}0&{}0&{}0\\ 0&{}\sigma ^2_{Q1}&{}0&{}0\\ 0&{}0&{}\sigma ^2_{Q2}&{}0\\ 0&{}0&{}0&{}\sigma ^2_{Q2}\\ \end{array} \right] , \;\; R=\sigma _R^2 I_2\) | \(Q=\left[ \begin{array}{cccc} \sigma ^2_{Q1}&{}0&{}0&{}0\\ 0&{}\sigma ^2_{Q1}&{}0&{}0\\ 0&{}0&{}\sigma ^2_{Q2}&{}0\\ 0&{}0&{}0&{}\sigma ^2_{Q2}\\ \end{array} \right] , \quad R=\sigma _R^2 I_2\) |