Observer
The MCSimPython.observer package contains a set of observers for DP applications. These
are not necessarly easy to use out of the box.
Nonlinear Observers
- class MCSimPython.observer.nonlinobs.NonlinObs3dof(dt, wc, wo, lambd, T, M, D, lambda_w=0.03)
Kalman Filters
- class MCSimPython.observer.ekf.EKF(dt, M, D, Tp, x0=array([0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]), P0=array([[0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]]))
Implementation of Extended Kalman Filter (EKF) for CSAD.
\[\begin{split}\hat{x} = \left[ \begin{array}{c} \zeta \\ \eta\\ b \\ \nu \end{array} \right]\;\; \zeta \in \mathbb{R}^6\; \eta, \nu, b \in \mathbb{R}^3\end{split}\]- Reference:
Fossen (2021), Marine craft hydrodynamics and motion control, ch 13.
- property EKF_gain
Kalman gain: Used to balance the contributions of the predicted state estimate and the measurement data in the updated estimate.
- Returns:
K – Kalman gain (dim = 15x3)
- Return type:
array_like
- corrector(y)
Corrector: Used to refine the predicted state estimate from the predictor, based on the obtained measurements. The function checks for signal loss (no measurement).
- Parameters:
y (array_like) – Measurements (3DOF)
implemented (To be) –
----------------- –
dt. (Check if measurement is given. Can also be made with modulus operator if measurement freq is different from) –
- initialize_constant_matrices(Tp)
Initialize following matrices:
\[ \begin{align}\begin{aligned}\begin{split}A_w = \left[ \begin{array}{cc} 0_{3x3} & I_{3x3} \\ -\omega^2 & -2\zeta\omega \end{array}\right]\end{split}\\\begin{split}E = \left[\begin{array}{cc} 0_{3x3} & 0_{3x3} \\ E_w & 0_{3x3} \\ 0_{3x3} & 0_{3x3} \\ 0_{3x3} & E_b \\ 0_{3x3} & 0_{3x3} \\ \end{array} \right]\end{split}\\\gamma = \Delta t E\\H = \left[ \begin{array}0_{3x3} & I_{3x3} & I_{3x3} & 0_{3x6} \end{array} \right]\\\begin{split}B = \left[\begin{array}0_{12x3} \\ M^{-1} \\ \end{array} \right]\end{split}\end{aligned}\end{align} \]- Parameters:
Tp (float) – Sea state peak period
implemented (To be) –
----------------- –
Tuning must be done
Can add wave spectrum properties as input in tuning? Tp, Damping, Kw_i
Use np.block() in _Aw
- predictor(tau)
Predictor: Used to estimate the state of the system at the next time step based on the current state estimate and the system dynamics model.
- Parameters:
tau (array_like) – Control input (3DOF) [X, Y, N]
- set_tuning_matrices(Q, R)
Customize tuning matrices in the EKF
- Parameters:
Q (array_like) – (6,6)-dim array stating covariance in model uncertainty
R (array_like) – (6,6)-dim array stating covariance in measurement uncertainty
- Return type:
N/A
- state_function(x, tau, noise=array([0., 0., 0., 0., 0., 0.]))
State function
\[ \begin{align}\begin{aligned}\dot{x} = A(x) + B\tau + Ew = f(x, u, w),\\\begin{split} A(x) =\left[\begin{array}A_w \xi \\ R_z(\psi)\nu \\ 0_{3\times 1} \\ -M^{-1}D\nu + M^{-1}R_z{\psi}^{\top} b \end{array}\right]\end{split}\end{aligned}\end{align} \]- Parameters:
x (array_like) – State vector (Dim = 15)
tau (array_like) – Control vector [X Y N]
noise (array_like) – Modelled white noise, set to zero in a deterministic EKF. (Dim=6)
- Returns:
f – x_dot (Dim = 15)
- Return type:
array_like
- state_function_jacobian()
phi = eye(15) + dt * del(f)/del(x)
where
\[\begin{split}\frac{\partial{f}}{\partial{x}} = \left[\begin{array}{cccc} A_w & 0_(6x3) & 0_(6x3) & 0_(6x3) \\ 0_(3x6) & \partial{f_2} & 0_(3x3) & R(psi) \\ 0_(3x6) & 0_(3x3) & 0_(3x3) & 0_(3x3) \\ 0_(3x6) & \partial{f_4} & M^{-1}R(\psi)^\top & -M^{-1}D \end{array}\right]\end{split}\]- Returns:
phi – Discretized jacobian of system dynamics (Dim = 15x15)
- Return type:
array_like
- update(tau, y)
Update: Update function to be called at every timestep during a simulation. Calls the predictor and corrector.
- Parameters:
tau (array_like) – Control Parameters (3DOF)
y (array_like) – Measured position (3DOF)
implemented (To be) –
----------------- –
Error checks?
Asynchronous measurements?
Set y to nan if no measurement