# Particle Filter for IMU tilt angle and bias estimation from Kalman Filter models

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, $$$$x_k = A x_{t-1},$$$$ and a corresponding motion uncertainty matrix $$Q$$. Similarly, I have a measurement model, $$$$z_t = C x_t,$$$$ 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: $$$$x \sim p(x_t ~ | ~ x_{t-1}, u_t).$$$$ Then, how do I assign a weight to each particle? That is, how do I evaluate this step of the algorithm $$$$w = p(z_t ~ | ~ x_{t}),$$$$ 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.

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.

• 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? – Shrinivas Iyengar Dec 30 '20 at 0:02
• 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})$ – Octopuscabbage Dec 31 '20 at 0:13
• Chapter 4.2 discusses the particle filter. Chapter 5 and 6 discuss computing these probabilities. – Octopuscabbage Dec 31 '20 at 0:15
• 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. – Shrinivas Iyengar Jan 14 at 14:16
• 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). – Shrinivas Iyengar Jan 14 at 14:28