1
$\begingroup$

I'm having troubles implementing a UKF during the calculation of $\overline{\mu_t}$, specifically in this step: $$\overline{\mu_t} = \sum_{i=0}^{2n}w_m^{[i]}\chi_t^{*[i]}$$

My problem I'm facing is that $\overline{\mu_t}$ seems to not be close to the values of the sigma points at all, and I have no idea why.

My state is composed of $x, y, v, a, \theta, \dot{\theta}$. Here is one example of an iteration: $$\mu_{t-1} = \begin{bmatrix}2.7 \\ 8.3e-05 \\ 1 \\ 0.2 \\ 3.2e-15 \\ 1e-9 \end{bmatrix}\\ \Sigma_{t-1} = \begin{bmatrix} 0.038& 7.6e-07& 0.013& 0.0018& -1.7e-09& 1.5e-11 \\ 7.6e-07& 0.99& 4e-07& 1.5e-07& 1& 8e-10 \\ 0.013& 4e-07& 0.005& 0.00095& 1.1e-17& 4.7e-12 \\ 0.00187& 1.5e-07& 0.00095& 0.00036& -1.8e-15& 5.7e-13 \\ -1.7e-09& 1& 1.1e-17& -1.8e-15& 1.1& -1.9e-23 \\ 1.5e-11& 8e-10& 4.7e-12& 5.7e-13& -1.9e-23& 1e-08 \\ \end{bmatrix}$$

The $x$ values of the sigma points after the transition model: \begin{bmatrix} 2.82606 & 2.85113 & 2.8251 & 2.82614 & 2.82606 2.82603 & 2.82606 & 2.801 & 2.8251 & 2.82599 & 2.82606 & 2.82603 & 2.82606 \end{bmatrix}

For my weights, I chose $\alpha$ such that weights would be integers, since I was worried that this was a floating point error: $$w_m^{[i]} = \begin{bmatrix}-384\\32\\32\\32\\32\\32\\32\\32\\32\\32\\32\\32\\32\end{bmatrix}$$

However, calculating $\overline{\mu_t}$ yields $-0.064$, which is evidently wrong. Wolfram alpha confirms that this is not a floating point error. I have no idea where I might be going in the calculation here.

Any idea how I would go about solving this problem?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.