Covariance time update for the unicycle model

42 Views Asked by At

Consider the following state vectors $$r_k := \begin{bmatrix}\xi_k & \eta_k\end{bmatrix}' \quad p_k := \begin{bmatrix}\theta_k & l_k^1 & l_k^2\end{bmatrix}' \quad u_k :=\begin{bmatrix} v_k & \omega_k \end{bmatrix}'$$ where $'$ denotes the transpose operator, and the following discrete state-space motion model $$\begin{cases} r_{k+1} & =r_k+T R(p_k) u_k +w_k^r \\ p_{k+1} & = p_k + T A_p^u u_k + w_k^p \\ u_{k+1} & = u_k +w_k^{u} \end{cases}$$

where $T$ is a constant scalar, while $$R(p_k):= \begin{bmatrix} \cos \theta_k & 0 \\ \sin \theta_k & 0\end{bmatrix} \qquad A_p^u := \begin{bmatrix} 0 & 1 \\ 0 & 0 \\ 0 & 0 \end{bmatrix}$$

and $w_k^r$, $w_k^p$, $w_k^u$ are white noises. Let's suppose the vectors $r_k$, $p_k$, $u_k$, $w_k^r$, $w_k^p$, $w_k^u$ be mutually uncorrelated with each other. It follows that the covariance matrix for $p_{k+1}$ is $$\Sigma_{k+1}^p :=\mathbb{E}[p_{k+1}p_{k+1}'] = \Sigma_k^p + T^2 A_p^u \Sigma_k^u (A_p^u)' +\Sigma _k ^{w^p}$$ and, in the same way $$\Sigma_{k+1}^u = \Sigma_k^u +\Sigma _k ^{w^u}$$

question: in the computation of $\Sigma_{k+1}^r$ I don't know how to treat the term $$T^2\mathbb{E}[R(p_k)u_k u_k' R(p_k)']$$ since the matrix $R(p_k)$ is not constant but depends on the random variable $\theta_k$. Is it possible to obtain a simple expression for this term? Maybe by assuming some additional assumption.

observation: if $\theta_k$ would be deterministic, then $$T^2\mathbb{E}[R(p_k)u_k u_k' R(p_k)']=T^2 R(p_k) \Sigma_k^u R(p_k)'$$ but this is not the case. This result is valid under the assumption of $p_k$ and $u_k$ uncorrelated?

Maybe, but I'm not sure, if we assume $p_k$ be independent from $u_k$, and if we call $\bar{p}_k:=\mathbb{E}[p_k]$, then $$T^2\mathbb{E}[R(p_k)u_k u_k' R(p_k)']=T^2 R(\bar{p}_k) \Sigma_k^u R(\bar{p}_k)'$$ since the expected value for the product of independent random variables is the product of the singles expected values for the random variables.

1

There are 1 best solutions below

0
On

Maybe you can try the Unscented Transformation (UT) which is utilized in the UKF.

Let $x_k=[r_k',p_k',u_k']'$ be the state vector of the considered system. The UT can be applied to estimate the mean and covariance of $x_{k+1}$ given $p(x_k)$ which is usually approximated by a Gaussian distribution.

Once the covariance of $x_{k+1}$ is estimated, the covariance matrix for $p_{k+1}$ is ready.