Batch Least Square Estimator

Problem

Let us consider simple two-dimensional problem of howitzer. A person fires a ball with canon at an angle \(\theta\) and initial velocity \(v_0\). Our engineer standing at distance \(d\) can see the bullet for specific period, he takes multiple measurements using a radar, note that these measurements \(\rho\) are straight line (line of sight) distance from measuring device to bullet. As we know, it is impossible to measure true quantity as there will be uncertainty (noise) associated with any measurement denoted by \(\sigma\). Let us consider we know angle \(\theta\) but do not have idea of initial velocity \(v_0\) and gravity \(g_0\) so the goal is to find out what will be the value of \(v_0\) and \(g_0\) from given measurements.

To solve this problem with Least Square Estimator we need to first propogate state and observation eqation.

The problem of parabolic projectile is trivial and has solution: $$ \vec{x} = \vec{v_0}t+\frac{1}{2} \vec{a} t^2$$ thus the state propogation can be done directly by substituting values of time \(t\) and we can easily compute how bullet moves in space with respect to time. Note that generaly engineering problems are in the form of Ordinary Diffrential Equations and can be solved using numirical integration methods such as Runge-Kutta method or adaptive Runge-kutta as in case of matlab ode45 solver.

In cartetian (2D) space, components of \(\vec{x}\) are \(x\) and \(z\) while \(u = v_0 cos(\theta)\) and \(w= v_0 sin(\theta)\) are the initial veocity components along \(x\) and \(z\) direction respectively gives state propogation eqation. Simulation below shows projectile of bullet with

\(v_0=80m/s \quad \theta=45^{\circ} \quad g_0=9.81m/s^2\)

here true trajectory of projectile is shown in green color, and measurements represented as yellow markers and measurements are computed as $$\rho = \sqrt{(x-d)^2+z^2}$$ and unit normal noise \(\sigma_{\rho}\) is added to simulated measuremets.

Solution

In order to find initial values \( X = [v_0 \quad g_0]\) associated with provided mesuremets+noise we need to minimize the deviation \(y\) between obseved mesuremets (data taken from radar) and computed measremets (computing measuremets by propogating state). $$y=\rho-\rho*$$

Mapping matrix \(H\) of dimention [No of states x No of measuremets] transforms state vector in to measurement vector. $$ H = \begin{bmatrix} \tilde{H}_1 \\ \tilde{H}_2 \\ \vdots \\ \tilde{H}_m \end{bmatrix} $$ $$ \tilde{H_i} = \frac{\partial \rho}{\partial X} = \begin{bmatrix} \frac{cos(\theta)(x_i - d) t_i+sin(\theta)z_i t_i}{\rho_i} & \frac{-t_i^2 z_i}{ 2\rho_i} \end{bmatrix} $$ Matrix \(\tilde{h}\) (jaccobian) is computed by parialy diffrentiating measurement vector with respect to state \([v_0 \quad g_0]\). State daviation \(\Delta X\) and observation(measurment) daviation y are related by mapping matrix \(H\) as $$ y = H \Delta X $$ in above relation we know y and H, \(\Delta X\) can be computed by multiplying y with inverse of \(H\). But shape of mapping matrix is not guaranteed to be square, pehapse it is always a rectangular matrix with number of rows much higher than number of column. This problem can be solved by taking psudoinverseof \(H\) to get Least Square Update. $$ \Delta X = H^T (HH^T)^{-1} y $$

Step 1: Generate problem by changing sliders bellow.

Velocity \(v_0\) [m/s] \(\quad\)Measurement Uncertanity \(\sigma_{\rho}\) [m]
Angle \(\theta\) [deg] \(\quad g_0\) = 9.81 [m/s2]

Step 2 : Initialize Estimator by guessing initial conditions.

Initial Guess Velocity \(v_0\) [m/s]
Initial Guess Gravity \(g_0\) [m/s2]

Click Least Square Update till residuals are converged.

Results

Observation Deviation

** Try updating values and see how solution converges or diverges depending on initial guess **

Regards,
Siddharth.