yu-xiaoxian / probabilistic_robotics_exercises Goto Github PK

I think the covariance matrix $\Sigma_{t-1}$ should be updated at each time $t$.

$$ \begin{equation} p(x_t | u_t, x_{t-1}) \sim N(A_t x_{t-1}, A_t\Sigma_{t-1} A_t^T+R) \end{equation} $$

But your update is doing like below:

$$ \begin{equation} p(x_t | u_t, x_{t-1}) \sim N(A_t x_{t-1}, A_t\Sigma_0 A_t^T+R) \end{equation} $$