0
$\begingroup$

I want to track an object in a global Cartesian frame, denoted $G$. The object has a local frame, denoted $L$. The object is controlled by a velocity command $v$, defined in its local frame. This "world" is a 2D space as below:

enter image description here

In this world, the object can only translate (no rotation), and the local axes and global axes are aligned. Therefore, I can track the object using a regular Kalman filter.

I define the state as $x_t = \begin{bmatrix}x_t\\y_t\end{bmatrix}$ where $x$ and $y$ are the positions in the global frame. The control velocity is defined as $v_t = \begin{bmatrix}a_t\\b_t\end{bmatrix}$, where $a$ and $b$ are the local velocities defined in the local frame. The state transition matrix is defined as $A = \begin{bmatrix}1&0\\0&1\end{bmatrix}$ and the input matrix is defined as $B = \begin{bmatrix}\Delta t&0\\0&\Delta t\end{bmatrix}$, where $\Delta t$ is the time step.

So overall, my prediction stage of the Kalman filter is:

$\begin{bmatrix}x_{t+1}\\y_{t+1}\end{bmatrix} = \begin{bmatrix}1&0\\0&1\end{bmatrix} \begin{bmatrix}x_t\\y_t\end{bmatrix} + \begin{bmatrix}\Delta t&0\\0&\Delta t\end{bmatrix} \begin{bmatrix}a_t\\b_t\end{bmatrix}$

But now I want to allow the object to rotate, about the axis going into the screen. So, my "world" now looks like this:

enter image description here

And now, the axes of the two frames are not aligned, so I cannot use the state transition model I defined above. So, to account for this in the Kalman filter, I do the following.

I take the current estimate of the object's pose, and create a transformation matrix to represent this frame. Let us denote this $T_{GL}$, which transforms a vector from the object's local frame to the global frame. Then, I take the velocity vector $v_L$, which is currently defined in the local frame, and transform to the global frame as follows: $v_G = T_{GL} v_L$.

Once this is done, I have the control velocity defined in the global frame, instead of the local frame. And so then I just use this velocity in the Kalman filter defined in the first example above. My intuition is that once the velocity is transformed to the global frame, I can use the simple state transformation model defined above, because the axes of the velocity and the global frame are now aligned.

But I am rather confused now. It seems like I have effectively implemented the Extended Kalman Filter (EKF), because the relationship between the local velocity and the state update is actually non-linear, due to the rotation. The EKF involves linearising around the current estimate, and I am effectively doing this by computing the transformation matrix for the current estimation of the object's pose. However, when I read about the EKF, the maths is much more complex. It involves Taylor series expansions and partial derivatives, whereas all I am doing is transforming a vector from the estimated local frame to the global frame.

Have I actually implemented an EKF, unknowingly? Or is there something about my implementation that is fundamentally wrong?

Thank you!

Improve this question
$\endgroup$
1
$\begingroup$

You have not accidentally implemented an Extended Kalman filter. All you are essentially doing is preprocessing the data with a known quantity $T_{GL}$ into your global frame. You therefore removed any indication of rotation and therefore your math is still happening in the global frame which allows you to continue to use your Kalman filter implementation.

Improve this answer
$\endgroup$
11
  • $\begingroup$ Thanks for your answer. Actually, $T_{GL}$ is not known; it is derived from my current estimate of the state. The state contains the Cartesian position and Euler rotation, and $T_{GL}$ is then a matrix representation of this. But what I am most confused about is why we even need the EKF when we can just do a coordinate transform in this way. This post (tinyurl.com/vk5nsel) says that you cannot do a transformation from Polar to Cartesian coordinates, since the mapping is non-linear, and therefore you need to use the EKF. Isn't this the same in my case, since rotations are involved? $\endgroup$ – Karnivaurus Jan 29 '20 at 10:56
  • $\begingroup$ Ahh ok. Yes you will now need an EKF. The reason you need an EKF is that you can no longer express your state transition matrices as a linear update(matrix math) as $T_{GL}$ is going to be composed of cos, and sin. Given the state $[x,y,\theta]$ try to rewrite your prediction step with matrices. You will find that you can't do it. $\endgroup$ – edwinem Jan 29 '20 at 20:45
  • $\begingroup$ I understand that $T_{GL}$ is composed of cos and sin in general, and so in general the prediction step is a non-linear function. However, what I am doing is taking the current state estimate, and then calculating $T_{GL}$ at this particular point. Then, $T_{GL}$ is just a matrix of numbers. So I can use it to transform a velocity from the local frame to the global frame, and hence use this velocity to update the state in the global frame. Would this be ok? $\endgroup$ – Karnivaurus Jan 29 '20 at 21:41
  • $\begingroup$ But in order to compute $T_{GL}$ at this point you also need to estimate the $\theta$ which you said is not known. If you can get $T_{GL}$ from some external source then it is probably not kosher, but you could probably continue to use just a Kalman filter. It just won't properly fuse the uncertainty. $\endgroup$ – edwinem Jan 30 '20 at 17:02
  • 1
    $\begingroup$ He explicitly takes care of that with the "Debiasing of the measurements". $\endgroup$ – edwinem Jan 31 '20 at 0:19

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

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