# Extended Kalman Filter The problem with standard Kalman filter is that it assumes linear state transitions and measurements. In reality, state transitions and measurements are rarely linear, e.g. a robot that moves with constant transnational and rotational velocity will move in a circular path, which is not linear. The *extended Kalman filter* relaxes the linearity assumption. We suppose that state transition probability and measurement probability are governed by nonlinear functions. $$ x\_{t} = g(u\_{t}, x\_{t-1}) + \epsilon\_{t} \\;\\ z\_{t} = h(x\_{t}) + \delta\_{t} $$ The function $$g$$replaces the matrices $$A\_{t}$$ and $$B\_{t}$$, and the function $$h$$ replaces the matrix $$C\_{t}$$. Unfortunately, with arbitrary non-linear functions, the belief is no longer a Gaussian. In fact, performing the belief update exactly is usually impossible in this case. The EKF (extended Kalman filter) needs to calculate a Gaussian approximation to the true belief via Monte Carlo methods. ## Linearization The key idea underlying the EKF approximation is called **linearization**. If we were to use Monte Carlo methods, we'd have to pass hundred thousands of points to $$g$$followed by the computation of their mean and covariance. This is extremely inefficient. We need a way to hack around it and approximates $$g$$ and $$h$$ as linear functions using first order Taylor expansion. ### Taylor Expansion Taylor expansion constructs a linear approximation to a function $$g$$ from $$g$$'s value and slope. The slope is given by the following expression. $$ g\prime(u\_{t}, x\_{t-1}) = \frac{\partial g}{\partial x\_{t-1}} $$ The value and slope of $$g$$are dependent on the arguments $$u\_{t}$$ and $$x\_{t-1}$$. We should choose the most probable states to perform Taylor expansion with. Since the most probable state of $$X\_{t}$$ is $$\mu\_{t-1}$$, $$g$$ is approximated by its values at $$\mu\_t{-1}$$ and $$u\_{t}$$. $$ g(u\_{t}, x\_{t-1}) \approx g(u\_{t}, \mu\_{t-1}) + g\prime(u\_{t}, \mu\_{t-1})(x\_{t-1} - \mu\_{t-1}) = g(u\_{t}, \mu\_{t-1}) + G\_{t}(x\_{t-1} - \mu\_{t-1}) $$ Notice that $$G\_{t}$$ is a n by n matrix, with n denoting the dimension of the state. This matrix is often called the Jacobian. The value of Jacobian is dependent on $$u\_{t}$$ and $$\mu\_{t-1}$$, hence the matrix is time dependent. EKF implements the exact same linearization for the measurement function $$h$$. The calculation must perform after prediction state, hence $$\overline{\mu}\_{t}$$. $$ h(x\_{t}) \approx h(\overline{\mu}*{t}) + h\prime(\overline{\mu}*{t})(x\_{t} - \overline{\mu}*{t}) = h(\overline{\mu}*{t}) + H\_{t}(x\_{t} - \overline{\mu}\_{t}) $$ ## EKF Algorithm Now if we put everything together, we have the EKF algorithm. Equation (1) performs the prediction steps. $$ \tag{1} \overline{\mu}*{t} = g(u*{t}, \mu\_{t-1}) \\;\\ \overline{\Sigma}*{t} = G*{t}\Sigma\_{t-1}G^{T}*{t} + R*{r}t $$ Equation (2) performs the Kalman gain calculation. $$ \tag{2} K\_{t} = \overline{\Sigma}*{t}H*{t}^{T}(H\_{t}\overline{\Sigma}H\_{t}^{T} + Q\_{t})^{-1} $$ Equation (3) performs the measurement incorporation steps. $$ \mu\_{t} = \overline{\mu}*{t} + K*{t}(z\_{t} - h(\overline{\mu}*{t})) \\;\\ \Sigma*{t} = (I - K\_{t}H\_{t})\overline{\Sigma}\_{t} $$ Finally, $$ \text{return};\mu\_{t}, \Sigma\_{t} $$ --- # Agent Instructions: Querying This Documentation If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question. Perform an HTTP GET request on the current page URL with the `ask` query parameter: ``` GET https://calvinfeng.gitbook.io/probabilistic-robotics/basics/gaussian-filters/02-extended-kalman-filter.md?ask= ``` The question should be specific, self-contained, and written in natural language. The response will contain a direct answer to the question and relevant excerpts and sources from the documentation. Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.