Fundamentals of parameter estimation - Part III

This is the continuation to Fundamentals of parameter estimation - Part II. Exercise 3/8 from my optimal estimation course. The focus is on random vectors and unbiased linear MMSE estimation.

At the end of this exercise I should understand insights about the concept of covariance matrices and about unbiased linear MMSE estimation.

Context

Prior knowledge

I have a ship. The parameter vector to estimate is the position vector of that ship $\mathbf{x}=[xy{]}^T$. The prior knowledge, obtained via dead reckoning, is captured as a prior expectation ${\mu }_{\mathbf{x}}$ and a covariance matrix which expresses the prior uncertainty that we have about the position $C_{\mathbf{x}}$.

Measurement

In order to increase the accuracy, the navigator of the ship measures the direction $\phi$ of a beacon, e.g. lighthouse, relative to the ship as in the Figure below.

The beacon has a known reference position ${\mathbf{x}}_0$. The line of sight is defined by the position of the beacon and by the measured direction $\theta$. The compass reading gives $\theta =\phi +△\theta$. The following equation defines the line of sight in the $(\xi ,\eta )$ plane:

$$x_0\sin \theta -y_0\cos \theta =\xi \sin \theta -\eta \cos \theta$$

The measurement model

The relation between the ship’s true position $\mathbf{x}=(x,y)$ and the true bearing $\phi$ is:

$$x_0\sin \phi -y_0\cos \phi =x\sin \phi -y\cos \phi$$

or, by substituting $\phi =\theta -△\theta$

$$x_0\sin (\theta -\Delta \theta )-y_0\cos (\theta -\Delta \theta )=x\sin (\theta -\Delta \theta )-y\cos (\theta -\Delta \theta )$$

The relation between the ship’s true position $\mathbf{x}$ and the observed direction $\theta$ is nonlinear. To get a linear approximation, we apply a truncated Taylor series expansion to the sine and cosine functions:

$$\sin (\theta -\Delta \theta )\approx \sin \theta -\Delta \theta \cos \theta$$ $$\cos (\theta -\Delta \theta )\approx \cos \theta +\Delta \theta \sin \theta$$ $$\Downarrow \text{after some rearrangements}$$ $$x_0\sin \theta -y_0\cos \theta \approx x\sin \theta -y\cos \theta +\Delta \theta \left((x_0-x)\cos \theta +(y_0-y)\sin \theta \right)$$

Since $\theta \approx \phi$, the factor $(x_0-x)\cos \theta +(y_0-y)\sin \theta$ almost equals $(x_0-x)\cos \phi +(y_0-y)\sin \phi$. The latter equals the distance $d$ between beacon and ship. Therefore:

$$x_0\sin \theta -y_0\cos \theta \approx x\sin \theta -y\cos \theta +d\Delta \theta$$

This can be written in the form $z=H\mathbf{x}+v$ with the following definitions $\{\begin{array}{l}z=x_0\sin \theta -y_0\cos \theta \\ H=[\sin \theta -\cos \theta ]\\ v=d\Delta \theta \end{array}$

The distance $d$ is unknown, but can be estimated from prior knowledge ${\mu }_{\mathbf{x}}$ of the ship’s position and the position of the beacon: $d\approx ||{\mathbf{x}}_0-{\mu }_{\mathbf{x}}||$. Assuming the measurement of the bearing has an uncertainty of ${\sigma }_{\Delta \theta }$, the standard deviation of $v$ is ${\sigma }_v=d{\sigma }_{\Delta \theta }$ [radians].

The Case

Physical units are Nautical miles (Nm).

Uncertainty regions and principal axes

For normal distributions $p(\mathbf{x})=\frac{1}{\sqrt{(2\pi {)}^N|C_x|}}\exp \left(-\frac{(\mathbf{x}-{\mu }_x{)}^T{C}_x^{-1}(\mathbf{x}-{\mu }_x)}{2}\right)$, the equation for the contour simplifies to: $(\mathbf{x}-{\mu }_x{)}^T{C}_x^{-1}(\mathbf{x}-{\mu }_x)=k^2\text{ with }k=1,2,3$.

The eigenvectors and eigenvalues are solutions of $C_x\mathbf{v}=\lambda \mathbf{v}$ and the corresponding scaling factors are $a_m=\sqrt{{\lambda }_m}$

---

First topic: Determine the eigenvalues and eigenvectors of $C_{\mathbf{x}}$ and draw the associated uncertainty region.

1.1 Generate a set of points on a circle with unit radius. The centre of the circle is positioned at the origin

1.2 Scale the $x$ and $y$ coordinates of these points in accordance with the scaling factors $a_0$ and $a_1$. The resulting points form an ellipse with the right shape, but not with the right orientation and position.

So based on the Figure 5 above, I need to extract the $a_0$ and $a_1$ scaling factors of the ellipse defined by $C_{\mathbf{x}}$. The corresponding scaling factors are $a_m=\sqrt{{\lambda }_m}$.

1.3 Rotate the set of points in accordance with the direction of the principal axes. The eigenvector-matrix is a rotation matrix.

This is really just a no-brainer, since the eigenvector-matrix is in itself the Rotation Matrix I need to apply.

1.4+1.5 Shift the whole set to the position determined by ${\mu }_{\mathbf{x}}$. Plot the curve defined by the resulting set of points.

Again, I simply add to each axis the values from ${\mu }_{\mathbf{x}}$.

---

Second topic: Add the line of sight to the Figure. From the context, the uncertainty of the measured bearing is the standard deviation ${\sigma }_{\Delta \theta }$. The range $[\theta -{\sigma }_{\Delta \theta },\theta +{\sigma }_{\Delta \theta }]$ defines an uncertainty region in the shape of a 2D cone. Visualize this cone in the graph by adding two dashed lines.

The line of sight is a line starting from the beacon position ${\mathbf{x}}_0$ going in the direction of the measured bearing $\theta$. So I must simply apply the equation:

$$y_{\text{los}}-y_0=m(x_{\text{los}}-x_0)$$

where $m=\tan (\theta )$.

For the upper and lower bounds of the LoS (the 2D cone), I can simply reapply the formula like this:

$$y=y_0+\tan (\theta \pm {\sigma }_{\Delta \theta })(x-x_0)$$

In the Figure above, in the left plot, the lines do not intersect the uncertainty region if I consider the compass reading of $\theta =35$. This would indicate that the prior position estimate and the compass measurement are pointing at slightly different locations (so the reading of the compass is off). To see what the true bearing should be, I applied $atan2$ between the bearing and the prior estimate, and I get the result of $\sim 41.63$ degrees. In this case, the line of sight would pass straight through the ship’s position in the right plot.

---

Third topic: The linearized measurement function replaces the cone by a bar (i.e. two parallel lines). The width of this bar is $2{\sigma }_v$. Calculate it and show the results.

According to the document, ${\sigma }_v=d{\sigma }_{\Delta \theta }$, where $d=||x_0-{\mu }_x||$ is the euclidean distance between the bearing and the prior position estimate. According to the calculations, the initial width of the bar is $\sim 4.2033Nm$.

Although $\theta$ is the real measurement, I can use $z$ as a derived measurement instead.

$$z={\mathbf{x}}_0\sin (\theta )-{\mathbf{y}}_0\cos (\theta )$$

Okk, but what is $z$? What does it represent?

While the actual physical measurement is the bearing angle $\theta$, its relationship to the ship’s position is non-linear. To make this usable for linear estimation, the measurement model is linearized using a Taylor series expansion. This process groups the known variables—the beacon’s position $(x_0,y_0)$ and the measured angle $\theta$ - into a single known scalar value $z$.

• Geometrically, the absolute value of z represents the shortest, perpendicular distance from the origin (0, 0) to the measured line of sight. Because it is a signed value, the positive or negative sign simply indicates which side of the origin the line falls on.

Basically, the true relationship is non-linear and I linearize it through the standard linear format $z=Hx+v$. I will need it in the unbiased linear MMSE estimator.

The linearized bar consists of two parallel lines defined by

$$x\sin (\theta )-y\cos (\theta )=z\pm {\sigma }_v$$

After rearranging, I get $y=x\tan (\theta )-\frac{z\pm {\sigma }_v}{\cos (\theta )}$

According to the plot, the results do make sense, since the cone and the bar are approximately equal in width near the ship’s position (they actually overlap, since the dashed line of the cone is no longer visible), which is where the linearization is valid. Further away from that, the approximation becomes less accurate.

---

Fourth topic: Determine the derived measurement $z$, the measurement matrix $H$, and the Kalman Gain matrix. The covariance matrix of the measurement noise is $C_v={\sigma }_v^2$. Next, calculate the unbiased linear MMSE estimate of the position and the corresponding (error) covariance matrix.

For the first part, I already had to compute the derived measurement $z$ in the last question, and its value is $z\sim -24.5576$ which makes sense. The minus sign signals that the side on which the shortest perpendicular falls on the line of sight is to the left of the origin. The actual distance would be $z=24.5576Nm$.

Since $H=[\sin \theta -\cos \theta ]$, the actual values of the measurement matrix would be $H=[0.5736,-0.8192]$. It maps the 2D position to the scalar measurement.

The Kalman Gain $K=C_{\mathbf{x}}{H}^T{\left(\begin{array}{c}H{C}_{\mathbf{x}}{H}^T+C_v\end{array}\right)}^{-1}$ (taken from eq. 3.33 from the book) weights how much to trust the measurement versus the prior knowledge. The actual values are $K\approx [0.419,-0.8625]$.

The updated estimate ${\mu }_{\text{post}}={\hat{\mathbf{x}}}_{\text{ulMMSE}}(z)={\mu }_{\mathbf{x}}+K(z-H{\mu }_{\mathbf{x}})$ depends mostly on the innovation $z-H{\mu }_{\mathbf{x}}$. The Kalman Gain transforms the innovation into a correction term that represents the knowledge that we have gained from the measurements.

When you invert a covariance matrix, you get the information matrix. Just a reminder.

The updated covariance $C_e={\left(\begin{array}{c}{C}_{\mathbf{x}}^{-1}+H^T{C}_V^{-1}H\end{array}\right)}^{-1}$ (taken from eq 3.44 from the book) represents the reduced uncertainty after incorporating the measurement. The actual values are $C_e\approx \left[\begin{array}{c}10.41225.0316\\ 5.03168.174\end{array}\right]$.

---

Fifth topic: Draw the uncertainty region of the estimate. That is, plot the posterior mean and covariance matrix.

Now, based on the updated information, the ship’s updated uncertainty region falls within the designated bounds of the line of sight. An interesting idea is that the bearing measurement only constrains the position perpendicular to the line of sight. Along the line of sight, distance remains uncertain, so the ellipse stretches in that direction (the red uncertainty region). Visibly, the Kalman gain K determined how much weight to give the measurement versus the prior. Since ${\sigma }_v$ was relatively small, the measurement was trusted and the uncertainty collapsed significantly in the perpendicular direction. Due to the innovation being nonzero, meaning the prior mean was not on the line of sight, the estimate got pulled onto it.

---

Sixth topic: Repeat questions 2 to 4 a number of times, but with varying values of ${\sigma }_{\Delta \theta }$ and explain what you see.

The term ${\sigma }_{\Delta \theta }$ directly influences the width of the linearized bar width and the covariance matrix of the measurement noise. Therefore, if the bearing uncertainty increases, then the update would take the measurement less into consideration, since the Kalman Gain has it in the denominator and the updated covariance matrix computes the error term based on its inverse.

However, since the uncertainty increases, that also increases the change of the initial guess to fall more and more within the linearized bar width. The updated position ${\mu }_{\text{post}}$ is more than likely to fall within the bounds, but the uncertainty also increases. This suggests that the lower the bearing uncertainty, the better and more accurate will the updates be, and a narrower space for uncertainty.

• An interesting observations is that as ${\sigma }_{\Delta \theta }\to \infty$, the posterior converges towards the prior. That happens because $K\to 0$ and $C_{\mathbf{x}\_\text{post}}\to C_{\mathbf{x}}$, meaning the measurement holds no value and no influence to the update.

• As ${\sigma }_{\Delta \theta }\to 0$, there is no real uncertainty region, because it would mean we would trust the measurement completely and the posterior ellipse collapses onto the line of sight.

---

Seventh topic: Repeat question 2 up to 4 a number of times, but with varying $C_{\mathbf{x}}$ by $\alpha$.

• If $\alpha$ increases, that means a larger prior uncertainty, which leads the Kalman Filter to trust the measurement much more. While the posterior will be placed inside the bar width, the uncertainty region grows bigger, which still translates to possible errors.

• As $\alpha$ decreases, that translates into trusting the prior more. Therefore, the posterior will incline towards ${\mu }_{\mathbf{x}}$ and not ${\mu }_{\text{post}}$.