2
$\begingroup$

I'm new to Kalman filtering and state estimation and I'd like some guidance on EKFs.

Currently, I'm trying to use a linear prediction model coupled with nonlinear measurements to estimate the state of an object. My state vector ($\bar{x}$) is:

\begin{bmatrix}x\\y\\v_x\\v_y\end{bmatrix}

and my state model is (with no process noise ($W_k$) or process noise covariance ($Q_k$)): $$ \bar{x}_{k_p} = A\bar{x}_{k-1} \ + B\begin{bmatrix} a_x \\ a_y \end{bmatrix} $$

$$ P_{k_p} = AP_{k-1}A^T $$ where $$ A = \begin{bmatrix} 1 & 0 & \Delta T & 0 \\ 0 & 1 & 0 & \Delta T \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix} $$ and $$ B = \begin{bmatrix} \frac{1}{2}\Delta T^2 & 0 \\ 0 & \frac{1}{2}\Delta T^2 \\ \Delta T & 0 \\ 0 & \Delta T\end{bmatrix} $$

I have simulated some measurement data with Gaussian noise. The measurements are outputted as $\begin{bmatrix}r\\\theta\end{bmatrix}$, and this is the graph of the generated data:

simulated measurements from a sensor outputting in polar coordinates

To account for the polar coordinates, I was going to use a function $h(z)$ that transformed from polar to cartesian coordinates in my state update equation $\bar{x}_k = \bar{x}_{k_p} \ + K_k(h(\bar{z}_k)-\bar{x}_{k_p})$? This seemed like the simple thing to do, but this post does say that if I did so, my filter wouldn't work as the polar to cartesian transform isn't a linear transform.

To resolve this, I want to use an EKF to linearize the measurements, and after reading a few articles online, my gain calculation and update step are as follows:

$$ K_k = P_kJ(\bar{z}_k)^T \ (J(\bar{z}_k)P_kJ(\bar{z}_k)^T + R)^{-1} $$

$$ \bar{x}_k = \bar{x}_{k_p} \ + K_k(Z_k - H\bar{x}_{k_p}) $$

$$ P_k = (I - K_k J(\bar{z}_k))P_{k_p} $$

where $K_k$ is the Kalman gain, $\bar{z}_k$ is the measurement vector outputted by the sensor, and $J(\bar{z}_k)$ is a Jacobian to linearize the polar coordinate measurements. These are all at time $k$. In addition, my prediction equations are the same linear equations as shown above.

I used $x = r \cos(\theta)$ and $y = r \sin(\theta)$ to derive the following Jacobian: $$ J(r,\theta) = \begin{bmatrix} \frac{\partial x}{\partial r} & \frac{\partial x}{\partial \theta} \\ \frac{\partial y}{\partial r} & \frac{\partial y}{\partial \theta} \\ \frac{\partial v_x}{\partial r} & \frac{\partial v_x}{\partial \theta} \\ \frac{\partial v_y}{\partial r} & \frac{\partial v_y}{\partial \theta} \\ \end{bmatrix} = \begin{bmatrix} \cos(\theta) & -r\sin(\theta) \\ \sin(\theta) & r\cos(\theta) \\ 0 & 0 \\ 0 & 0 \end{bmatrix} $$

Would this Jacobian and model be the correct way to go about my problem? Thanks for your help!

P.S. I'm super new to this so if some of my lingo is wrong or weird, if I'm missing something or something makes no sense, or if you have some tips to better my understanding, please let me know. Thanks!

EDIT: I converted the polar values to the cartesian plane with $x = r\cos(\theta)$ and $y=r\sin(\theta)$, and then graphed the $x$ and 4y$ values. This is the result: Conversion from polar to cartesian

Obviously, something seems to be working, but I don't know/understand why, especially if the transform isn't linear. If someone could help my understanding of this as well, I'd truly appreciate it.

$\endgroup$
  • $\begingroup$ You may drop the Jacobian altogether. Just have a look on UKF (See my answer below with remarks from article doing Polar <-> Cartesian coordinate change). I will be much easier to implement (You have many references). $\endgroup$ – Royi Aug 21 '18 at 5:55
  • $\begingroup$ If you are interested in tracking understanding linearization is important. The UKF has good performance but is simpler. It also incorporates discontinuities and range constraints more directly. Linearization including higher order produces equations which can be useful in of themselves if you want to reduce the order of your system. $\endgroup$ – Stanley Pawlukiewicz Aug 21 '18 at 7:06
  • $\begingroup$ A UKF computes something like 3,5,7...function evaluations per dimension. While this isn’t typically a problem but in an application constrained by heat and battery life, a linearization may require fewer arithmetic operations and that can be an advantage. People still use alpha beta fixed gain filters. You also might be employed by an organization that has a lot of legacy applications and you need the proper knowledge to support life cycle activities $\endgroup$ – Stanley Pawlukiewicz Aug 21 '18 at 7:40
  • $\begingroup$ I updated my question with the right derivation of the Jacobian. You made a mistake on that (If I understood your model correctly). $\endgroup$ – Royi Aug 21 '18 at 19:51
  • 1
    $\begingroup$ I added UKF solution in my Code. See my answer for the link. Enjoy... $\endgroup$ – Royi Aug 24 '18 at 13:33
2
$\begingroup$

Update

If I understood your model, you have a model of Constant Velocity in 2D (Cartesian Coordinate System).
While your measurement are in Polar Coordinate System.

Pay attention that your measurement function is:

$$ h \left( x, y, {v}_{x}, {v}_{y} \right) = \begin{bmatrix} \sqrt{ {x}^{2} + {y}^{2} } \\ {\tan}^{-1} \left( \frac{y}{x} \right ) \end{bmatrix} $$

Hence you Jacobian becomes:

$$\begin{aligned} {J}_{h} \left( x, y, {v}_{x}, {v}_{y} \right) & = \begin{bmatrix} \frac{\partial \sqrt{ {x}^{2} + {y}^{2} } }{\partial x} & \frac{\partial \sqrt{ {x}^{2} + {y}^{2} } }{\partial y} & \frac{\partial \sqrt{ {x}^{2} + {y}^{2} } }{\partial {v}_{x}} & \frac{\partial \sqrt{ {x}^{2} + {y}^{2} } }{\partial {v}_{y}} \\ \frac{\partial {\tan}^{-1} \left( \frac{y}{x} \right ) }{\partial x} & \frac{\partial {\tan}^{-1} \left( \frac{y}{x} \right ) }{\partial y} & \frac{\partial {\tan}^{-1} \left( \frac{y}{x} \right ) }{\partial {v}_{x}} & \frac{\partial {\tan}^{-1} \left( \frac{y}{x} \right ) }{\partial {v}_{y}} \end{bmatrix} \\ & = \begin{bmatrix} \frac{x}{ \sqrt{ {x}^{2} + {y}^{2} } } & \frac{y}{ \sqrt{ {x}^{2} + {y}^{2} } } & 0 & 0 \\ -\frac{y}{{x}^{2} + {y}^{2}} & \frac{x}{{x}^{2} + {y}^{2}} & 0 & 0 \end{bmatrix} \end{aligned}$$

The function of the measurement is the one connecting your state vector into the measurement and not the other way around.

Let's look on Wikipedia EKF Model:

enter image description here

enter image description here

In you case $ F $ is constant is the model is linear.
What's in Wikipedia called $ H $ is the $ J $ I derived above.
Dimension wise, all is perfectly defined.

Implementation

I implemented a general Kalman Filter Iteration with support for Extended Kalman Filter (With option for Numeric Calculation of the Jacobian).
I also added option to Unsecented Kalman Filter (UKF) Iteration, so you will be able to compare.

Here is a result in with the same model as yours:

enter image description here

The full code is available on my StackExchange Signal Processing Q51386 GitHub Repository.

24/08/2018: I added UKF implementation which is pretty general so you could use it in various models.

Original Answer

Converting coordinate system is really the main reason the Extended Kalman Filter was invented.

Yet I will tell you tip, it doesn't work well in those cases.
If you use Non Linear Transformation use something that will both make things easier and better (Yea, usually it doesn't work like that, but in this case it does) - Use the Unscented Kalman Filter (UKF) which is based on the Unscented Transform.
Once you utilize that there is no need to derive the Jacobian.
All needed is to apply the non linear function $ n $ times (On each Sigma Point).

It is easy to see that linearization doesn't work well for propagating the mean and the covariance in many (Most) cases.
The UKF directly approximate the calculation of the integration of the non linear function which calculates the mean and covariance.

It will make things easier as you'll be able to skip the linearization step and only know the coordinate transformation function.

In modern tracking we usually stay away from EKF and utilize methods which better approximate the integrals of the first 2 moments propagation.
The most common ones are the UKF and GHKF (Those are called Sigma Points Kalman filters).
Their generalization is the Particle Filter which in most cases is over kill.

Update
Have a look at EKF / UKF Maneuvering Target Tracking using Coordinated Turn Models with Polar/Cartesian Velocity.

From their conclusion:

We have shown a range of coordinated turn (CT) models using either Cartesian or polar velocity and how to use them in a Kalman filtering framework for maneuvering target tracking. The results of the conducted simulation study are in favor of polar velocity. This confirms the results of the previous study [11] and extends it to the case of varying target speed. For polar CT models, the performance in terms of position RMSE of the predicted state appears to be comparable for EKF and UKF. As the UKF does not require the derivation and implementation of Jacobians it might be more straightforward to implement. The RMSE provided by the Cartesian velocity EKF and UKF turned out slightly worse. Interestingly, the sensitivity of the RMSE with respect to the noise parameters was decreased by using EKF2 and UKF in the Cartesian case. This, in addition to the simpler implementation and lower computational cost of UKF over EKF2 results in a recommendation for UKF if Cartesian CT models are preferred.

Basically telling you, don't bother with Jacobians, just use the simpler UKF.

Another comparison is made at Implementation of the Unscented Kalman Filter and a simple Augmentation System for GNSS SDR receivers with:

As can be seen, UKF implementation does not require linearization (state transition function and measurements functions are directly applied to sigma points) and it can also work in presence of discontinuities. The prediction only consists of linear algebra operations. All such advantages are fundamental for minimizing computational load in an SDR implementation.

While the classical Kalman Filter implies the propagation of n components for the state vector and n2 /2+n/2 components for the Covariance matrix, the UKF requires the propagation of 2n+1 sigma points only.

Furthermore, UKF is more insensitive to initial conditions with respect to EKF. It has been demonstrated that UKF rapidly converge also in presence of an initial position error of several Kilometers.

$\endgroup$
  • $\begingroup$ I’m glad you mentioned particle filters after I did., but you didn’t explain why something works or doesn’t work or why it is a good idea or not. Your answer is incomplete and doesn’t answer the OP’s question. $\endgroup$ – Stanley Pawlukiewicz Aug 21 '18 at 5:14
  • $\begingroup$ You know he asked about his derivation of the Jacobian while you didn't even mention it. I suggested him that instead of using Jacobian he should neglect the linearization entirely as it doesn't work in many cases and work with Sigma Points filters. Hence I did answer his question while you didn't. $\endgroup$ – Royi Aug 21 '18 at 5:49
  • $\begingroup$ I mentioned the truncated Taylor series which includes second order filters. Now you are adding SDR applications while he presented a kinematic motion problem which Blackman covers in depth. That really helps a beginner. The simpler is better argument works if simpler is better. The problem with papers making comparisons is that the comparison is typically to a straw man. Was the comparison to every type of EKF, no. I got nothing against using an UKF. I think that the relationship to Gaussian quadrature is missing. $\endgroup$ – Stanley Pawlukiewicz Aug 21 '18 at 6:43
  • $\begingroup$ The real problem with linearization is when the nonlinearity is discontinuous. I wouldn’t call it a transformation because it isn’t invertable Sigma points are very similar to quadrature, specifically those based on Hermite polynomials. Keep on adding to your answer. Cut and paste, I thought you had a problem with that $\endgroup$ – Stanley Pawlukiewicz Aug 21 '18 at 6:53
  • $\begingroup$ UKF is much simpler to someone who starts with Non Linear settings than the linearization process. Given that it also performance better it is time for EKF to be left alone. The problem with linaerization isn't only the discontinuities it is imply that it distort the PDF in a way which makes the Mean and covariance irrelevant. $\endgroup$ – Royi Aug 21 '18 at 6:55
1
$\begingroup$

UPDATE

It looks like we are all stumbling forward

This section is wrong, and I appreciate being corrected.

First your Jacobian is incorrect for the polar to cartesian transform,  from

https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant 

$$
\begin{align}
x &= r \cos \varphi ; \\
y &= r \sin \varphi .
\end{align}$$

$$\mathbf J_{\mathbf F}(r, \varphi) = \begin{bmatrix}
  \dfrac{\partial x}{\partial r} & \dfrac{\partial x}{\partial\varphi}\\[1em]
  \dfrac{\partial y}{\partial r} & \dfrac{\partial y}{\partial\varphi} \end{bmatrix}
= \begin{bmatrix}
  \cos\varphi & - r\sin \varphi \\
  \sin\varphi &   r\cos \varphi \end{bmatrix}
$$

$R^2 \rightarrow R^2$ Jacobians are square.

the stumble forward part

Unless your measurement angles are limited to the first and second quadrant, I believe the measurement equation should be:

$$ g\left((r=\sqrt{x^2+y^2},\theta=\text{atan2}(y,x)\right)=\left| \begin{array}{c} r \\ \theta \end{array}\right|_{r=\sqrt{x^2+y^2} \\ \theta=\text{atan2}(y,x)} $$

According to https://en.wikipedia.org/wiki/Atan2 the partial derivatives are the same as for $\text{atan}(y/x)$ so the Jacobian that @Royi is correct, but the measurement equation he gives is not correct for $-\pi\le \theta \le \pi$.

End Update

The post says the filter might not work. This is a situation where you need to experiment and there are no hard and fast rules. There are Extended Kalman Filters as opposed to is an EKF. There are filters that are first order and filters that are second order. There are iterated filters.

One difficulty with using nonlinearities directly in the state variable evolution is the difficulty of the nonlinear integration update to propagate the mean. Most old books will try to modify Euler Integration by using terms of the Taylor Series. You can also use an ODE solver like Runge Kutta integration.

The second difficulty is propagation of variance. One can use Taylor Series terms or you can use a UKF or particle filter by estimating the spread by numerical integration. For the UKF, quadrature or importance sampling for the particle filter.

In mixed systems where just the measurement or just the state is nonlinear, you can treat the linear part as linear which is known as Rao Blackwellization .

The KF/EKF doesn’t require process noise but you have a kinematic state variable motion model which is kind of dull without process noise. Measurement noise is required for the filter to work. If you don’t have measurement noise you use a state observer and don’t need a filter.

There are many papers on the kind of problem you describe. Some with a nonlinear measurement model and some with a nonlinear state model. Using inverse range as a coordinate system is very common, particularly in bearing only measurement cases.

The book by Blackman https://scholar.google.com/scholar?hl=en&as_sdt=0%2C47&q=blackman+multiple-target+tracking&oq=Blackman+multi

Covers coordinate systems and transformation issues in detail . The book is expensive but should be available in a good library. There are many ways to approach the problem.

An EKF can be much more than a linearization. A UKF actually uses the nonlinearity directly and so can a EKF with ODE integration.

It should be pointed out that step size has an effect.

IMHO, It should be called a smorgasbord Kalman Filter.

To answer your question, using the nonlinearity directly to propagate the mean ( predicted measurement) is often OK. On the measurement side there isn’t an integration. The covariance update is the tricky part. Linearization is the most common approach. The UKF uses the nonlinearity directly so it would be odd to say it was a “bad idea “

some edits:

Even in the linear state, linear measurement scenario, kinematic tracking state variable models don't include aerodynamic drag which is approximated by a term proportional to the velocity squared. Process noise covers modeling errors as well as the perturbative random forces that a body may encounter.

Process noise is also important because it controls the relative significance of a measurement and weighs the state variable evolution to either the prediction or the measurement. Without process noise a filter can't follow deviations from the state model. Your filter can track. There is a technique called Interacting Multiple Models (IMM) that is suitable to large deviations in the state model but I'm not going to mention it further.

$\endgroup$
  • $\begingroup$ I liked the fact that my answer got you extend your question beyond just pasting some references. But it doesn't mean you need to -1 others or copy others answers. No, even iterated EKF doesn't propagate the mean and covariance well in many cases. So for modern tracking there is no need to use it. $\endgroup$ – Royi Aug 21 '18 at 3:52
  • $\begingroup$ If I copied someone’s answer that would be one thing. References are important so if you don’t like them, it would seem to be your problem $\endgroup$ – Stanley Pawlukiewicz Aug 21 '18 at 5:18
  • $\begingroup$ @StanleyPawlukiewicz "The KF/EKF doesn’t require process noise but you have a kinematic state variable motion model which is kind of dull without process noise. Measurement noise is required for the filter to work. If you don’t have measurement noise you use a state observer and don’t need a filter." You're right. In fact, I forgot to mention that I had modeled this problem with a linear measurement model and included process noise. The filter worked perfectly in that case. $\endgroup$ – bparikh Aug 21 '18 at 9:54
  • $\begingroup$ @StanleyPawlukiewicz "Most old books will try to modify Euler Integration by using terms of the Taylor Series. You can also use an ODE solver like Runge Kutta integration." Would you be able to elaborate a bit more on this? In my case, would such a technique be used in the measurement step to approximate a linear equation of the data at that timestep? $\endgroup$ – bparikh Aug 21 '18 at 10:01
  • $\begingroup$ @bparikh, that really applies to the nonlinear state evolution. I apologize to you because most of what is going on here isn't really focused on your problem. I'll make some edits (over the next few days) to my answer to clarify what I said. A central issue is that there is really no strong theoretical justification for the EKF's near optimality, and given all the variations of the EKF, it comes to to what is better not best for a particular application. I like Blackman's book because the multitarget problem is the more realistic kinematic tracking problem. $\endgroup$ – Stanley Pawlukiewicz Aug 21 '18 at 15:16

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.