Vehicle Dynamics model

$\left[\begin{array}{c}{x}^{}\\ v^{}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}x\\ v\end{array}\right]+\left[\begin{array}{c}0\\ a_{\text{t}\text{r}\text{u}\text{e}}(t)\end{array}\right]+\left[\begin{array}{c}0\\ (t)\end{array}\right]$

Acceleration measurement model

$a_{\text{m}\text{e}\text{a}\text{s}}(t)=a_{\text{t}\text{r}\text{u}\text{e}}+b+{}_a(t)$

Measurement model

Range from the $i^{th}$ station

$r_i(k)=\sqrt[\phantom{2}]{(x_i-x{)}^2+h_i^2}+$

$u(k)$

where $x_i,h_i$ are the position and the height of the $i^{th}$ radar. The position and altitude of each radar are provided in the table below

The truck accelerates from $0\frac{m}{s^2}$ to $0\mathrm{.}05\frac{m}{s^2}$ in first $100s,$ remains at $0\mathrm{.}05\frac{m}{s^2}$ acceleration for next $100s$ and deccelerates to $0\frac{m}{s^2}$ in the next

Simulate the measured acceleration and the measured ranges based on the above specifications for $300$s at every $1$ second. Develop an EKF estimator to

estimate the position, velocity and the accelerometer bias. Show the performance of the implemented EKF using proper figures with error, standard

deviations and residuals$\mathrm{.}100s$

$"""\_$summary$\_$

This module contains the classes used to implement various estimation algorithms.

$"""$

import numpy as np

from scipy.linalg import expm, inv

from scipy.integrate import odeint

class state:

$"""\_$summary$\_$

This class is used to store the state of the system at a given time.

x $-$ state vector

P $-$ state covariance matrix

t $-$ time

z $-$ predicted observation

res $-$ residual

innovation$\_$cov $-$ innovation covariance matrix

$"""$

def $\_\_$init$\_\_($self$,**$kwargs$)$:

if len$($kwargs$)==0$:

self.x $=$ None

self.P $=$ None

self.t $=$ None

self.z $=$ None

elif len$($kwargs$)==3$:

self.x $=$ kwargs$[\text{'}$x$\text{'}]$

self.P $=$ kwargs$[\text{'}$P$\text{'}]$

self.t $=$ kwargs$[\text{'}$t$\text{'}]$

else:

self.x $=$ kwargs$[\text{'}$x$\text{'}]$

self.P $=$ kwargs$[\text{'}$P$\text{'}]$

self.t $=$ kwargs$[\text{'}$t$\text{'}]$

self.z $=$ kwargs$[\text{'}$z$\text{'}]$

self.res $=$ kwargs$[\text{'}$res$\text{'}]$

self.innovation$\_$cov $=$ kwargs$[\text{'}$innovation$\_$cov'$]$

class kalman$\_$filter:

$"""\_$summary$\_$

This class is used to implement the Kalman filter algorithm.

model $-$ model of the system

Q $-$ process noise covariance matrix

R $-$ measurement noise covariance matrix

state$0-$ initial state of the system

$"""$

def $\_\_$init$\_\_($self$,$ model, Q$,$ R$,$ state$0,$ dT$)$:

$"""\_$summary$\_$

Args:

model $(\_$structure$\_)$:

model.dynamics $(\_$function$\_)$: $\_$system model$\_,$ dynamics needs to be compatible with odeint

model.Phi $(\_$function$\_)$: $\_$system jacobian$\_$

model.h $(\_$function$\_)$: $\_$observation model$\_$

model.H $(\_$function$\_)$: $\_$observation jacobian$\_$

Q $(\_$ndarray$\_)$: $\_$process noise covariance$\_$

R $(\_$ndarray$\_)$: $\_$measurement noise covariance$\_$

state$0(\_$state$\_)$: $\_$Initial state information$\_$

dT $(\_$scalar$\_)$: $\_$time step$\_$

$"""$

self.model $=$ model

self.Q $=$ Q

self.R $=$ R

self.state$\_$post $=$ state$0$

self.state$\_$priori $=$ state$()$

self.dt $=$ dT

def KF$\_$prediction$($self$)$:

$"""\_$summary$\_$

Kalman filter prediction step

$"""$

x $=$ self.state$\_$post.x

P $=$ self.state$\_$post.P

t $=$ self.state$\_$post.t

Phi $=$ self.model.Phi

x$\_$priori $=$ odeint$($self$.$model.dynamics, x$,[$t$,$ t$+$ self.dt$])[-1,$:$]$ #state propagation

P$\_$priori $=$ Phi.dot$($P$).$dot$($Phi$.$T$)+$ self.Q #covariance propagation

z$\_$priori $=$ self.model.h$($x$\_$priori$)$ #observation prediction

self.state$\_$priori.x $=$ x$\_$priori

self.state$\_$priori.P $=$ P$\_$priori

self.state$\_$priori.z $=$ z$\_$priori

self.state$\_$priori.t $=$ t$+1$

return

def KF$\_$correction$($self$,$ observation$)$:

$"""\_$summary$\_$

Args:

observation $(\_$vector$\_)$: $\_$Observation$\_$

$"""$

x$\_$priori $=$ self.state$\_$priori.x

P$\_$priori $=$ self.state$\_$priori.P

z$\_$priori $=$ self.state$\_$priori.z

H $=$ self.model.H

residue $=$ observation $-$ z$\_$priori

R $=$ self.R

if len$($R$.$shape$)>2$:

K $=$ P$\_$priori.dot$($H$.$T$).$dot$($inv$($H$.$dot$($P$\_$priori$).$dot$($H$.$T$)+$R$))$

self.state$\_$post.x $=$ x$\_$priori $+$ K$.$dot$($residue$)$

self.state$\_$post.P $=($np$.$identity$(2)-$K$.$dot$($H$)).$dot$($P$\_$priori$).$dot$(($np$.$identity$(2)-$K$.$dot$($H$)).$T$)\backslash$

$+$ K$.$dot$($R$).$dot$($K$.$T$)$

else:

K $=$ P$\_$priori.dot$($H$.$T$)*(($H$.$dot$($P$\_$priori$).$dot$($H$.$T$)+$R$)**-1)$

K $=$ K$.$reshape$($K$.$shape$[0],1)$

#pdb$.$set$\_$trace$()$

self.state$\_$post.x $=$ x$\_$priori $+($K$*$residue$).$reshape$($len$($x$\_$priori$))$ #state update

self.state$\_$post.P $=($np$.$identity$(2)-$K$.$do