2
$\begingroup$

I understand the functioning of Particle Filters from the book Probabilistic Robotics and the robotics course provided by Cyrill Stachniss.

I want to implement, from scratch, a particle filter to estimate the tilt angle $\theta$, angular velocity $\omega := \dot\theta$ and bias $b$ in one direction. I want to implement the most basic PF version as shown below. I have the mathematical model (that I can't post here for legal reasons) to do so when I was learning Kalman Filters in a University course. To explain in short, I have a process model, \begin{equation} x_k = A x_{t-1}, \end{equation} and a corresponding motion uncertainty matrix $Q$. Similarly, I have a measurement model, \begin{equation} z_t = C x_t, \end{equation} and the measurement covariance matrix $R$.

How do I go from this model to implementing the same in Particle Filters? PF requires to sample from the probabilistic state transition model: \begin{equation} x \sim p(x_t ~ | ~ x_{t-1}, u_t). \end{equation} Then, how do I assign a weight to each particle? That is, how do I evaluate this step of the algorithm \begin{equation} w = p(z_t ~ | ~ x_{t}), \end{equation} which requires evaluating the posterior for each particle $x_t$. I want to implement the most basic PF algorithm shown below, which I was able to do with Kalman Filters.

Particle Filter Algorithm from Probabilistic Robotics

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

The simplest way to construct $$x \sim p(x_t | x_{t-1}, u)$$ if you have $$x_t = Ax_{t-1}$$ is to just use $$x_t = Ax_{t-1} + \mathcal{N}(0,R)$$ where R is some process noise. You may way to sample from a more complex distribution.

Similarly, to construct $$p(z_t | x_t)$$ you can just use $$\mathcal{N}(Cx_t,Q)$$ (again you may want to use some more complex distribution)

$R$ is your process noise, $Q$ is your sensor noise.

As an addendum, Probabilistic Robotics has a whole chapter on constructing these probabilities for particle filters.

Improve this answer
$\endgroup$
5
  • $\begingroup$ thanks a lot. I still do not understand how would we compare $z_t$ to the expected measurement $C x_t$. $p(z_t | x_t)$ should give us a probability value for each hypothetical particle $x_t$. $\mathcal{N}(Cx_t,Q)$ does not incorporate the real measurement anywhere. By the way, I am in the process of reading Probabilistic Robotics in an unstructured way. Can you please tell me which chapter talks about constructing these probabilities? $\endgroup$
    – Shrinivas Iyengar
    Commented Dec 30, 2020 at 0:02
  • $\begingroup$ To calculate $P(z_t | x_t)$ given $\mathcal{N}(Cx_t,Q)$ you can use the normal distribution liklihood which would be calculated like $L(z_t) = (2\pi)^{k/2} det(Q)^{1/2} e^{1/2 (z_t - Cx_t)^T Q^{-1} (z_t - Cx_t}) $ $\endgroup$
    – Octopuscabbage
    Commented Dec 31, 2020 at 0:13
  • $\begingroup$ Chapter 4.2 discusses the particle filter. Chapter 5 and 6 discuss computing these probabilities. $\endgroup$
    – Octopuscabbage
    Commented Dec 31, 2020 at 0:15
  • $\begingroup$ Hi. So, I tried using $L(z_t)$ to assign weight to each particle, but somehow each particle has weight 0.0. I realised, through a conversation with someone, that $L(z_t)$ is a density function and that we'd need to integrate (in $[a, b]$) it to get the likelihood of being in that range. $\endgroup$
    – Shrinivas Iyengar
    Commented Jan 14, 2021 at 14:16
  • $\begingroup$ Additionally, I read Chapters 5 and 6. Chapter 5 gives a good idea of sampling from the state transition model. Though it only talks about motion models for mobile robots I could still understand how to use it in my case. BUT, Chapter 6 mainly only talks about models for range finders. There is no discussion of working with a given measurement model (which, I assume, is fair, given that it is a book for mobile robotics). $\endgroup$
    – Shrinivas Iyengar
    Commented Jan 14, 2021 at 14:28

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Not the answer you're looking for? Browse other questions tagged or ask your own question.