Assignment 2, Part 5

Problem 1

a)

A model for estimating the yaw angle, yaw rate and rudder bias as stated in [1] as Equations (13.44 - 13.46). This is a first order Nomoto model:

$$\begin{array}{rl}\dot{\psi }& =r\\ \dot{r}& =-\frac{1}{T}r+\frac{K}{T}(\delta -b)+w_1\\ \dot{b}& =w_2\end{array}$$

where the states are:

$$x={\left[\begin{array}{ccc}\psi & r& b\end{array}\right]}^T$$

The system matrices then becomes:

$$\begin{array}{rlr}& A=\left[\begin{array}{ccc}0& 1& 0\\ 0& -\frac{1}{T}& -\frac{K}{T}\\ 0& 1& 0\end{array}\right]& B=\left[\begin{array}{c}0\\ \frac{K}{T}\\ 0\end{array}\right]\\ & & \\ & C=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]& D=0\\ & & \\ & E=\left[\begin{array}{cc}0& 0\\ 1& 0\\ 0& 1\end{array}\right]& \end{array}$$

And the state-space model:

$$\begin{array}{rl}\dot{\mathbf{x}}& =\mathbf{Ax}+\mathbf{B}\delta +\mathbf{E}w\\ \mathbf{y}& =\mathbf{Cx}+\mathbf{D}\delta +\epsilon \end{array}$$

where $\epsilon$ is the zero-mean Gaussian measurement noise.

b)

The discretised system is as follows:

$$\begin{array}{rl}{\mathbf{A}}_d[k]& =\mathbf{I}+\mathbf{A}h=\left[\begin{array}{ccc}1& h& 0\\ 0& 1-\frac{h}{T}& -\frac{Kh}{T}\\ 0& h& 1\end{array}\right]\\ {\mathbf{B}}_d[k]& =\mathbf{B}h=\left[\begin{array}{c}0\\ \frac{Kh}{T}\\ 0\end{array}\right]\\ {\mathbf{C}}_d[k]& =\mathbf{C}=\left[\begin{array}{ccc}1& 0& 0\end{array}\right]\\ {\mathbf{D}}_d[k]& =\mathbf{D}=0\\ {\mathbf{E}}_d[k]& =\mathbf{E}h=\left[\begin{array}{cc}0& 0\\ h& 0\\ 0& h\end{array}\right]\end{array}$$

c)

A = [0 1 0;
	 0 -1/T -K/T;
	 0 1 0];
B = [0 K/T 0]';
C = [1 0 0];
D = 0;
E = [0 0;
	1 0;
	0 1];
Ad = eye(3) + A*h;
Bd = B*h;
Cd = C;
Dd = D;
E = E*h;
 
obs = [C; C*A;C*A*A];
disp(obs);
if rank(obs) == length(x_prd)
fprintf("System is fully observable\n");
else
fprintf("System is not observable\n");
end

>> observability
[1,    0,    0]
[0,    1,    0]
[0, -1/T, -K/T]
 
System is fully observable

The system is fully observable and all the states can be estimated.

.

Problem 2

a)

Noise added to yaw and yaw rate using normrnd(.) in matlab. Figure 1: Modelled noisy measurements

b)

Tuning the initial value of P has no effect on the system, as t converges quickly enough to the desired values. It is therefore initialised as $\mathbf{P}=0$. Setting $\mathbf{Q}$ much larger than $\mathbf{R}$ results in a very noise estimate, while the opposite results in much better estimates. This is expected as we have implemented measurements with noise and no process noise. Setting $\mathbf{Q}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$ and $\mathbf{R}=2000$ is therefore values that yields good noise suppression. The new Nomoto-model constants is:

$$\begin{array}{rl}& \\ T& =-99.4713\\ K& =-0.0049\end{array}$$

c)

When simulating the system with the noisy measurements as feedback it navigates more or less similarly to using proper state feedback, but the commanded rudder angle oscillates between the saturation limits very quickly. This is not a desired behaviour and would likely destroy the rudder over time. Figure 2: Map with noisy measurements feedback Figure 3: Rudder angle, yaw and yaw rates with noisy measurements feedback

d)

Simulating with the state estimates results in a much better rudder commands as shown in Figure 5, but the overall path does not improve much as shown in Figure 4. This can, however, be tuned in the PID-heading controller.

Figure 4: Map with state estimate feedback

Figure 5: Rudder angle, yaw and yaw rates with state estimate feedback . .

Problem 3

a)

The main intention of wave filtering is to filter out wave-induced disturbances from the measurements. The wave frequency (WF) disturbance component generate forces that have the same frequency as the waves [1]. Filtering this out leads to better estimation of the ship states.

b)

The main difference between model-based navigation and inertial navigation systems is that the linear accelerations and angular rates are modelled in model-based navigation and measured in INSes. The main drawback of not measuring the accelerations and rates is that it introduces greater uncertainty in the system, as they have to be calculated usually as derivates. The main advantage of a model-based system is that the IMU or similar sensor is not needed.

[1] Thor I. Fossen (2021) Handbok of Marine Craft Hydrodynamics. and Motion Control, Wiley