# Spring-mass problem: tracking based on range and range rate

Created on 01 January 2018

Example 4.8.2 Spring-mass problem
pg 216 in  Statistical Orbit Determination, Tapley, Born, Schutz.
pg 271 problem 15

@author: Ashiv Dhondea


```python
# Import generic libraries
import numpy as np
import math
```


```python
# Importing what's needed for nice plots.
import matplotlib.pyplot as plt
from matplotlib import rc
rc('font', **{'family': 'serif', 'serif': ['Helvetica']})
rc('text', usetex=True)
params = {'text.latex.preamble' : [r'\usepackage{amsmath}', r'\usepackage{amssymb}']}
plt.rcParams.update(params)
from mpl_toolkits.axes_grid.anchored_artists import AnchoredText
```

Object of mass $m$ is connected to two fixed walls by springs with spring constants $k_1$ and $k_2$. The sensor used to measure the range $\rho$ and range-rate $\dot{\rho}$ is located at a point $P$ which is at a height of $h$ on the wall to which spring $k_1$ is attached.

The initial position and velocity of the object are assumed to be $x_0 = 3.0~\mathrm{m}$ and $v_0 = 0.0~\mathrm{m/s}$.

The EOM (Equation of Motion) of the body is given by
\begin{equation}
\ddot{x} = - (k_1 + k_2)(x-\bar{x})/m = -\omega^2 (x-\bar{x})
\end{equation}
where $x$ is the position of the object, $\bar{x}$ is its static equilibrium position and $\omega$ is the angular frequency. $\bar{x}$ is to 0.


```python
# Parameters for the given problem
k1 = 2.5;
k2 = 3.7;
m = 1.5;
h = 5.4;
x0 = 3.0;
v0 = 0.0;

omega2 = (k1+k2)/m;
omega = math.sqrt(omega2);
```

From the EOM, it is inferred that an appropriate choice of state vector is
\begin{equation}
\mathbf{X} = 
\begin{bmatrix}
x \\
v
\end{bmatrix}
\end{equation}

The relationship between position $x$ and velocity $v$ easily leads to a differential equation
\begin{equation}
\frac{dx}{dt} = v
\end{equation}
while the relationship between velocity and acceleration leads to another DE.
\begin{equation}
\frac{dv}{dt} = -\omega^2 x
\end{equation}

These two DEs lead to:
\begin{equation}
\frac{d}{dt}\begin{bmatrix}
x \\
v
\end{bmatrix}
=
\begin{bmatrix}
v \\
-\omega^2 x
\end{bmatrix}
\end{equation}

The state space formulation is thus
\begin{equation}
\dot{\mathbf{X}} = 
\begin{bmatrix}
0 & 1 \\
-\omega^2 & 0
\end{bmatrix}
\mathbf{X} = \mathbf{A} \mathbf{X} 
\end{equation}

Function fn_A implements $\mathbf{A}$.

An analysis of the state space formulation leads to the state transition matrix $\Phi$ given by
\begin{equation}
\Phi =
\begin{bmatrix}
\cos (\omega t) & \frac{1}{\omega}\sin (\omega t) \\
-\omega \sin (\omega t) & \cos (\omega t)
\end{bmatrix}
\end{equation}

Check: the determinant of this STM is indeed 1.

Function fn_STM implements the STM.

The sensor measures the range and range-rate to the object.
The measurement vector $y$ is related to the state vector $x$ by the measurement function $g$.
\begin{equation}
y = \begin{bmatrix}
\rho \\
\dot{\rho}
\end{bmatrix}
=
\begin{bmatrix}
\sqrt{x^2 + h^2}\\
\frac{xv}/{\rho}
\end{bmatrix}
\end{equation}

Functin fn_G implements the measurement function $g$.

Both the range and range-rate are nonlinear functions of the system state. Therefore, the Jacobian matrix of the measurement function needs to be found for inclusion in the nonlinear tracking filter.
\begin{equation}
\tilde{\mathbf{H}}(\mathbf{X}) =
\begin{bmatrix}
\frac{x}{\rho} & 0 \\
\big( \frac{v}{\rho} -\frac{x^2 v}{\rho^3} \big) & \frac{x}{\rho}
\end{bmatrix}
\end{equation}

This is implemented in function fn_Htilde.


```python
# Define the functions given by the stated equations 
def fn_A(x_state,t,omega):
    # eqn 4.8.3
    A = np.array([[0.,1.],[-omega**2,0.]],dtype=np.float64);
    return A
    
def fn_G(x_state,h):
    # eqn 4.8.4
    rho = math.sqrt(x_state[0]**2+h**2);
    G = np.array([rho,x_state[0]*x_state[1]/rho],dtype=np.float64);
    return G

def fn_Htilde(x_state,h):
    # Jacobian matrix of G
    dx = np.shape(x_state)[0];
    x = x_state[0];v=x_state[1];
    rhovel = fn_G(x_state,h);
    dy=np.shape(rhovel)[0];
    rho = rhovel[0];#rhodot=rhovel[1];
    Htilde=np.zeros([dy,dx],dtype=np.float64);
    Htilde[0,0]=x/rho;
    Htilde[1,0]= v/rho - x**2*v/rho**3;
    Htilde[1,1]=x/rho;
    return Htilde
    
def fn_STM(t,omega):
    stm = np.zeros([2,2],dtype=np.float64);
    stm[0,0]=math.cos(omega*t);
    stm[0,1]=(1/omega)*math.sin(omega*t);
    stm[1,0]=-omega*math.sin(omega*t);
    stm[1,1]=math.cos(omega*t);
    return stm
```


```python
# Define the time step and time vector
delta_t = 1.0; # [s]
timevec = np.arange(0.,10.+delta_t,delta_t,dtype=np.float64);

# Dimensionality of the measurement vector
dy = 2; # 2 quantities measured
y_meas = np.zeros([dy,len(timevec)],dtype=np.float64); # array of measurements
# Load measurements
import pandas as pd 

dframe = pd.read_excel("sod_book_pg218_table_4_8_1_data.xlsx",sheetname="Sheet1")
dframe = dframe.reset_index()
data_time = dframe['Time'][0:len(timevec)+1]
data_range = dframe['Range'][0:len(timevec)+1]
data_range_rate = dframe['Range rate'][0:len(timevec)+1]

y_meas[0,:] = data_range;
y_meas[1,:] = data_range_rate;
```


```python
# a priori values
x_nom_ini = np.array([4.,0.2],dtype=np.float64); # a priori state vector 
P_ini = np.diag(np.array([1000.,100.],dtype=np.float64)); # a priori covariance matrix
dx = np.shape(x_nom_ini)[0]; # dimensionality of the state vector
delta_x_ini = np.zeros([dx],dtype=np.float64); # a priori perturbation vector

```


```python
def fn_BatchProcessor(delta_x,xnom,Pnom,timevec,y_meas,R_meas,omega,h):
    dy = np.shape(R_meas)[0];
    # Total observation matrix 
    TotalObsvMat = np.linalg.inv(Pnom)#np.zeros([dx,dx],dtype=np.float64);
    # total observation vector
    TotalObsvVec = np.dot(TotalObsvMat,delta_x)#np.zeros([dx],dtype=np.float64);
    # simulated perturbation vector
    delta_Y = np.zeros([dy],dtype=np.float64);
    Rinv = np.linalg.inv(R_meas);
    
    for index in range(len(timevec)):
        stm_nom = fn_STM(timevec[index],omega)
        x_state_nom = np.dot(stm_nom,xnom);
        H = fn_Htilde(x_state_nom,h)
        Hi = np.dot(H,stm_nom);
        HiT = np.transpose(Hi);
        Ynom = fn_G(x_state_nom,h);
        
        delta_Y = np.subtract(y_meas[:,index],Ynom);
        TotalObsvMat = TotalObsvMat + np.dot(HiT,np.dot(Rinv,Hi));
        TotalObsvVec = TotalObsvVec + np.dot(HiT,np.dot(Rinv,delta_Y));
        
    S_hat = np.linalg.inv(TotalObsvMat);
    delta_x = np.dot(S_hat,TotalObsvVec);
    x0_hat = xnom + delta_x;
    return x0_hat,delta_x,S_hat
```


```python
sd_rho = 1.;
sd_rhodot = 1.;
R_meas = np.diag(np.array([sd_rho**2,sd_rhodot**2],dtype=np.float64));
```


```python
num_iter = 4;
x0_hat = np.zeros([dx,num_iter],dtype=np.float64);
S_hat = np.zeros([dx,dx,num_iter],dtype=np.float64);
obsv_error = np.zeros([dy,num_iter],dtype=np.float64);


xnom_in = x_nom_ini;
Pnom_in = P_ini;
delta_x_in = delta_x_ini;
for i_iter in range (0,num_iter):
    x0_hat[:,i_iter],delta_x,S_hat[:,:,i_iter] = fn_BatchProcessor(delta_x_in,xnom_in,Pnom_in,timevec,y_meas,R_meas,omega,h);
    xnom_in = x0_hat[:,i_iter];
    delta_x_in = delta_x_in - delta_x;
    
     

print x0_hat[:,num_iter-1]
print S_hat[:,:,num_iter-1]

sigma_x0 = math.sqrt(S_hat[0,0,num_iter-1]);
sigma_v0 = math.sqrt(S_hat[1,1,num_iter-1]);
sigma_x0v0 = math.sqrt(S_hat[0,1,num_iter-1]);
print sigma_x0
print sigma_v0
print sigma_x0v0 
print math.sqrt(S_hat[1,0,num_iter-1]);
```

    [  3.00019485e+00   1.18181268e-03]
    [[ 0.16934803  0.01277539]
     [ 0.01277539  0.58448166]]
    0.411519179131
    0.764514003907
    0.113028287431
    0.113028287431


See page 219 in SOD book:
The state estimate x0_hat[:,num_iter-1] is identical to the estimate in the book.

The standard deviations are identical to those in the book. The correlation coefficient is incorrect.



```python

```

05d2fe7eab9687e8cbcc54efec067d1cc7f3a353
ipynb
Jupyter Notebook
examples/spring_mass_problem/Notebook_000_springmassproblem.ipynb
AshivDhondea/PyTrackingToolbox
817dca19335aa03808a5b4ff25cdce612c3ada0d
MIT
2019-11-12T23:07:16.000Z
2022-01-23T23:18:06.000Z
examples/spring_mass_problem/Notebook_000_springmassproblem.ipynb
AshivDhondea/PyTrackingToolbox
817dca19335aa03808a5b4ff25cdce612c3ada0d
MIT
examples/spring_mass_problem/Notebook_000_springmassproblem.ipynb
AshivDhondea/PyTrackingToolbox
817dca19335aa03808a5b4ff25cdce612c3ada0d
MIT
2019-11-05T14:41:24.000Z
2020-06-30T02:57:56.000Z
Qwen/Qwen-72B
1. YES
2. YES