1
$\begingroup$

If we consider the first-order Taylor expansion of a general nonlinear function at the operating point $x=x_0$, then we have the following,

$$f(x) \approx f(x_0) + \frac{\partial{f}}{\partial{x}}|_{x=x_0} (x - x_0)$$

If we now consider the linearization process of the EKF motion model equations with general nonlinear dynamics given by

$$\dot{x} = f(x,u)$$

then we find,

$$f(x,u) \approx f(x_0,u_0) + \frac{\partial{f}}{\partial{x}}|_{x=x_0,u=u_0} (x - x_0) + \frac{\partial{f}}{\partial{u}}|_{x=x_0,u=u_0} (u - u_0)$$

$$\dot{x} \approx f(x_0,u_0) + \frac{\partial{f}}{\partial{x}}|_{x=x_0,u=u_0} (x - x_0) + \frac{\partial{f}}{\partial{u}}|_{x=x_0,u=u_0} (u - u_0)$$ And in discrete form using Euler integration, $$x_{k+1} = x_{k} + f(x_0,u_0)\Delta t + \frac{\partial{f}}{\partial{x}}|_{x=x_0,u=u_0} (x - x_0) \Delta t + \frac{\partial{f}}{\partial{u}}|_{x=x_0,u=u_0} (u - u_0) \Delta t$$

However, whenever I see the EKF linearization process explained, the evaluation of the original function at the operating point, $f(x_0,u_0)$, and the differences from the operating point, $(x-x_0)$ and $(u-u_0)$, seem to be ignored. As such, we instead get,

$$x_{k+1} = x_{k} + \frac{\partial{f}}{\partial{x}}|_{x=x_0,u=u_0} x\Delta t + \frac{\partial{f}}{\partial{u}}|_{x=x_0,u=u_0} u \Delta t$$ $$x_{k+1} = x_{k} + A_k x_k \Delta t + B_k u_k \Delta t$$ where $A_k = \frac{\partial{f}}{\partial{x}}|_{x=x_0,u=u_0}$ and $B_k = \frac{\partial{f}}{\partial{u}}|_{x=x_0,u=u_0}$ which is equivalent to $$x_{k+1} = A_{d,k}x_k + B_{d,k} u_k$$ where $A_{d,k} = I+A_k\Delta t$ and $B_{d,k} = B_k\Delta t$ are the discrete-time state transition matrix and control input matrix, respectively.

I'm sure it has been well documented somewhere, by why are we allowed to only consider the operating point $(x_0, u_0)$ in the $A$ and $B$ Jacobians (and by extension the observation/measurement Jacobian used in the EKF's update step), and ignore $f(x_0,u_0)$ as well as replace $(x-x_0)$ with $x$ and $(u-u_0)$ with $u$?

$\endgroup$
  • $\begingroup$ Dumb answer. The solution to the discrete time transition matrix is trying to approximate e^(Adt) which I+Adt + 0.5A^2*dt^2 ... which is a maclaurin species which is a special case of the Taylor series where x0,u0 is equal to 0. $\endgroup$ – edwinem Dec 22 '19 at 8:22

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.