Linsys lab forberedelser dag 4

Resources: Kalman Filter

Problem 1

Given the following discrete-time model

$$\begin{array}{rl}x[k+1]& =A_{d}x[k]+B_{d}u[k]+w_d[k]\\ y[k]& =C_{d}x[k]+v_d[k]\end{array}$$

Where $w_d\sim \mathcal{N}(0,Q_d)$, $v_d\sim \mathcal{N}(0,R_d)$, and $x=\left[\begin{array}{cccccc}p& \dot{p}& e& \dot{e}& \lambda & \dot{\lambda }\end{array}\right]$. $Q_d$ will be used as a tuning variable, and $R_d$ will be experimentally estimated in the lab.

We remember how to discretize a system using Exact Discretization

Transclude of Exact-Discretization#^8febd7

Where $T=0.02s$

Transclude of Kalman-Filter#^141688

We first have (in continuous time):

$$\begin{array}{rl}\left[\begin{array}{c}\dot{p}\\ \ddot{p}\\ \dot{e}\\ \ddot{e}\\ \dot{\lambda }\\ \ddot{\lambda }\end{array}\right]& =\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 1\\ K_3& 0& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}p\\ \dot{p}\\ e\\ \dot{e}\\ \lambda \\ \dot{\lambda }\end{array}\right]+\left[\begin{array}{cc}0& 0\\ 0& K_1\\ 0& 0\\ K_2& 0\\ 0& 0\\ 0& 0\end{array}\right]\left[\begin{array}{c}{\tilde{V}}_s\\ V_d\end{array}\right]\\ y& =\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]\end{array}$$

Using Matlab we get the discrete system.

T = 0.002;
sys = ss(A,B,C,D);
sys_discrete = c2d(sys, T);
[A_d, B_d, C_d, D_d] = ssdata(sys_discrete);

By using the block in Simulink called “To Workspace”, we can extract $y$.

load("NAME_OF_SIMULINK_BLOCK.mat");
y_out = NAME_OF_SIMULINK_VARIABLE;
y_out(1, :) = []
R_d = cov(y_out');

This is done by remove the first column, and then transposing the matrix before we find the covariance matrix, $R_d$.