YA's Blog

Linearization of non-linear dynamics/observation in the EKF is a clever trick to use the KF equations. We approximate a new linear function from non-linear function around one point i.e. the mean of the Gaussian. However, there is still a better way to approximate it, say we approximate around multiple points. This is the inituition behind the powerful method called Unscented Kalman Filter (UKF). We take some points on source Gaussian and map them on target Gaussian after passing points through some non linear function and then we calculate the new mean and variance of transformed Gaussian. These special set of points are called "Sigma points". It can be very difficult to transform whole state distribution (computationally expensive) through a non linear function but it is very easy to transform some individual points. These sigma points are the representatives of whole distribution. Unscented Transform Consider a non-linear function $z = f(x)$ for a random variable $\math...

The Kalman Filter heavily exploits the fact that our dynamics/measurements are linear. However, typical robotics problems have non-linear dynamics or non-linear observation model or both. Extended-Kalman Filter (EKF) is a modification of the KF to handle such situations. EKF linearizes the dynamics and observation equations of a non-linear system at each timestep and then plugs them into the Kalman Filter formulae. Consider a non-linear dynamical system with non-linear observations, \begin{align*} x_{k+1} &= f(x_k, u_k) + \epsilon \tag{1.1}\\ y_k &= g(x_k) + \upsilon \tag{1.2} \end{align*} where noise vectors $\epsilon \sim N(0,R)$ and $\upsilon \sim N(0,Q)$ are defined similar to KF. The central idea of the Extended Kalman Filter (EKF) is to linearize a nonlinear system at each timestep $k$ using the Taylor series around the latest state estimate given by the Kalman Filter and use the resultant linearized dynamical system in the KF equations for the next timestep. Step...

Kalman Filter is by far the most important algorithm in robotics and it is hard to imagine running any robot without the Kalman fiter or some variant of it. Our goal is to compute the best estimate of the state after multiple observations i.e., $P(x_k | y_1, ..., y_k)$. Think of it as a problem where state $x_k$ is the position of a car (which could be stationary or moving) and observations $y_k$ give us some estimate of the this position. Using filtering, we want to find the probability of the car being at a location at time k given all previous observations. Consider a linear dynamical system with linear observations, \begin{align*} x_{k+1} &= A x_k + B u_k + \epsilon \tag{1.1}\\ y_{k} &= C x_k + \upsilon \tag{1.2} \end{align*} where $u_k$ denotes the control input at time step $k$ and noise vectors $\epsilon \sim N(0,R)$ and $\upsilon \sim N(0,Q)$. We assume that dynamics noise and observation noise are “white”, i.e., uncorrelated in time. Next, we assume that the i...

State Estimation is a very important problem in robotics wherein the probability distribution over the possible states of a robot is computed using the sensor readings (or observations). Let's look at the most basic linear state estimation problem where $X \in \mathbb{R}^{d}$ denotes the true state of a system. We would like to build an estimator for this state (which indicates our belief of the state) denoted by $\hat{X}$, which is a random variable. We would like the estimator to be unbiased, i.e., \begin{equation*} E[\hat{X}] = X \end{equation*} as it expresses the concept that if we were to measure the state of the system many times, say using many sensors or multiple observations from the same sensor, the resultant estimator is correct on average. The covariance of this estimator is denoted by $\Sigma_{\hat{X}}$. Note that a covariance matrix is a symmetric and positive semi-definite. Let's imagine that we have a sensor that gives us observations of the state as, \begi...