Skip to content
Snippets Groups Projects
Commit c7e604bb authored by Adam Friedrich Schrey's avatar Adam Friedrich Schrey
Browse files

Delete V10_kalman_filter.ipynb to replace it with V10_kalmanfilter_demonstration.ipynb

parent fa7baca5
No related branches found
No related tags found
1 merge request!1merge develop into master
Show whitespace changes
Inline Side-by-side
notebooks/V10_kalman_filter.ipynb deleted 100644 → 0
+ 0
− 763
View file @ fa7baca5
%% Cell type:markdown id: tags:
# <span style='color:OrangeRed'>V10 - Kalman Filter</span>
%% Cell type:markdown id: tags:
## <span style='color:Gray'>Beispiel #1 </span>
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
SFUNTMPL General M-file S-function template:
<br><br> With M-file S-functions, you can define you own ordinary differential
equations (ODEs), discrete system equations, and/or just about
any type of algorithm to be used within a Simulink block diagram.
The general form of an M-File S-function syntax is:
<code> [SYS,X0,STR,TS] = SFUNC(T,X,U,FLAG,P1,...,Pn) </code>.
What is returned by <code>SFUNC</code> at a given point in time, <code>T</code>, depends on the
value of the <code>FLAG</code>, the current state vector,<code> X</code>, and the current
input vector, <code>U</code>.
%% Cell type:markdown id: tags:
| Flag |Result | Description|
| :- | -: | :-: |
| 0 | [SIZES,X0,STR,TS] | Initialization, return system sizes in SYS, initial state in X0, state ordering strings in STR, and sample times in TS.|
| 1 | DX | Return continuous state derivatives in SYS.|
| 2 | DS | Update discrete states SYS = X(n+1)
| 3 | Y | Return outputs in SYS.
| 4 | TNEXT | Return next time hit for variable step sample time in SYS.
| 5 | | Reserved for future (root finding).|
| 9 | [] | Termination, perform any cleanup SYS=[].
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
Optional parameters, P1,...,Pn can be provided to the S-function and used during any <code>FLAG</code> operation.
When <code>SFUNC</code> is called with <code>FLAG</code> = 0, the following information
should be returned:
<code>SYS(1)</code> = Number of continuous states.
<br> <code>SYS(2)</code> = Number of discrete states.
<br><code>SYS(3)</code> = Number of outputs.
<br><code>SYS(4)</code> = Number of inputs.
Any of the first four elements in SYS can be specified
as -1 indicating that they are dynamically sized. The
actual length for all other flags will be equal to the
length of the input, <code>U</code>.
<br> <code>SYS(5)</code> = Reserved for root finding. Must be zero.
<br><code>SYS(6)</code> = Direct feedthrough flag (1=yes, 0=no). The s-function
has direct feedthrough if <code>U</code> is used during the <code>FLAG</code>=3
call. Setting this to 0 is akin to making a promise that
<code>U</code> will not be used during <code>FLAG</code>=3. If you break the promise
then unpredictable results will occur.
<br><code>SYS(7)</code> = Number of sample times. This is the number of rows in <code>TS</code>.
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
The state vectors, <code>X</code> and <code>X0</code> consists of continuous states followed
by discrete states.
<br> <code>X0</code> = Initial state conditions or [] if no states.
<br><code>STR</code> = State ordering strings which is generally specified as [].
<br><code>TS</code> = An m-by-2 matrix containing the sample time. (period, offset) information. Where m = number of sampletimes. The ordering of the sample times must be
<br> <code>TS</code> =<br> [0 0, : Continuous sample time.
<br>0 1, : Continuous, but fixed in minor step sample time.
<br> PERIOD OFFSET, : Discrete sample time where
<br> PERIOD greater than 0 & OFFSET smaller than PERIOD.
<br> -2 0 ]; : Variable step discrete sample time
<br>where <code>FLAG</code>=4 is used to get time of next hit.
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
There can be more than one sample time providing
they are ordered such that they are monotonically
increasing. Only the needed sample times should be
specified in <code>TS</code>. When specifying than one
sample time, you must check for sample hits explicitly by
seeing if
<code>abs(round((T-OFFSET)/PERIOD) - (T-OFFSET)/PERIOD)</code>
is within a specified tolerance, generally 1e-8. This
tolerance is dependent upon your model's sampling times
and simulation time.
You can also specify that the sample time of the S-function
is inherited from the driving block. For functions which
change during minor steps, this is done by
specifying <code>SYS(7)</code> = 1 and <code>TS</code> = [-1 0]. For functions which
are held during minor steps, this is done by specifying
<code>SYS(7)</code> = 1 and <code>TS</code> = [-1 1].
%% Cell type:code id: tags:
``` python
function [sys,x0,str,ts] = kalman1(t,x,u,flag,sigma,Ts,xo,ro)
switch flag,
% The following outlines the general structure of an S-function.
% Initialization:
case 0,
[sys,x0,str,ts]=mdlInitializeSizes(Ts,xo,ro);
% Derivatives:
case 1,
sys=mdlDerivatives(t,x,u);
% Update:
case 2,
sys=mdlUpdate(t,x,u,sigma);
% Outputs:
case 3,
sys=mdlOutputs(t,x,u);
% GetTimeOfNextVarHit:
case 4,
sys=mdlGetTimeOfNextVarHit(t,x,u,Ts);
% Terminate:
case 9,
sys=mdlTerminate(t,x,u);
% Unexpected flags:
otherwise
error(['Unhandled flag = ',num2str(flag)]);
end
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlInitializeSizes</code>
returns the sizes, initial conditions, and sample times for the S-function.
%% Cell type:code id: tags:
``` python
function [sys,x0,str,ts]=mdlInitializeSizes(Ts,xo,ro)
% Call simsizes for a sizes structure, fill it in and convert it to a sizes array.
% Note that in this example, the values are hard coded. This is not a recommended practice
% as the characteristics of the block are typically defined by the S-function parameters.
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 2;
sizes.NumOutputs = 1;
sizes.NumInputs = 1;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
% initialize the initial conditions:
x0 = [xo ro];
% str is always an empty matrix:
str = [];
% initialize the array of sample times:
ts = [Ts 0];
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlDerivatives</code>
returns the derivatives for the continuous states.
%% Cell type:code id: tags:
``` python
function sys=mdlDerivatives(t,x,u)
sys = [];
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlUpdate</code>
handles discrete state updates, sample time hits, and major time step
requirements.
%% Cell type:code id: tags:
``` python
function sys=mdlUpdate(t,x,u,sigma)
sys(2) = sigma*sigma*x(2)/(sigma*sigma+x(2));
K = sys(2)/(sigma*sigma);
sys(1) = x(1)+K*(u(1)-x(1));
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlOutputs</code>
returns the block outputs.
%% Cell type:code id: tags:
``` python
function sys=mdlOutputs(t,x,u)
sys(1) = x(1);
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code> mdlGetTimeOfNextVarHit</code>
returns the time of the next hit for this block. Note that the result is
absolute time. Note that this function is only used when you specify a
variable discrete-time sample time [-2 0] in the sample time array in
<code>mdlInitializeSizes</code>.
%% Cell type:code id: tags:
``` python
function sys=mdlGetTimeOfNextVarHit(t,x,u)
sys = [];
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code> mdlTerminate</code>
performs any end of simulation tasks.
%% Cell type:code id: tags:
``` python
function sys=mdlTerminate(t,x,u)
sys = [];
end
```
%% Cell type:markdown id: tags:
## <span style='color:Gray'>Beispiel #2 </span>
%% Cell type:code id: tags:
``` python
function [sys,x0,str,ts] = kalman2(t,x,u,flag,sigma,Ts,xo,ro)
switch flag,
% The following outlines the general structure of an S-function.
% Initialization:
case 0,
[sys,x0,str,ts]=mdlInitializeSizes(Ts,xo,ro);
% Derivatives:
case 1,
sys=mdlDerivatives(t,x,u);
% Update:
case 2,
sys=mdlUpdate(t,x,u,sigma);
% Outputs:
case 3,
sys=mdlOutputs(t,x,u);
% GetTimeOfNextVarHit
case 4,
sys=mdlGetTimeOfNextVarHit(t,x,u,Ts);
% Terminate:
case 9,
sys=mdlTerminate(t,x,u);
% Unexpected flags:
otherwise
error(['Unhandled flag = ',num2str(flag)]);
end
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlInitializeSizes</code>
returns the sizes, initial conditions, and sample times for the S-function.
%% Cell type:code id: tags:
``` python
function [sys,x0,str,ts]=mdlInitializeSizes(Ts,xo,ro)
% Call simsizes for a sizes structure, fill it in and convert it to a sizes array.
% Note that in this example, the values are hard coded. This is not a recommended practice
% as the characteristics of the block are typically defined by the S-function parameters.
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 2;
sizes.NumOutputs = 1;
sizes.NumInputs = 1;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
% initialize the initial conditions:
x0 = [xo ro];
% str is always an empty matrix:
str = [];
% initialize the array of sample times:
ts = [Ts 0];
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlDerivatives</code>
returns the derivatives for the continuous states.
%% Cell type:code id: tags:
``` python
function sys=mdlDerivatives(t,x,u)
sys = [];
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlUpdate</code>
handles discrete state updates, sample time hits, and major time step
requirements.
%% Cell type:code id: tags:
``` python
function sys=mdlUpdate(t,x,u,sigma)
sys(2) = sigma*sigma*x(2)/(sigma*sigma+x(2));
K = sys(2)/(sigma*sigma);
sys(1) = x(1)+K*(u(1)-x(1));
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlOutputs</code>
Return the block outputs.
%% Cell type:code id: tags:
``` python
function sys=mdlOutputs(t,x,u)
sys(1) = x(1);
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code> mdlGetTimeOfNextVarHit</code>
returns the time of the next hit for this block. Note that the result is absolute time. Note that this function is only used when you specify a variable discrete-time sample time [-2 0] in the sample time array in <code>mdlInitializeSizes</code>.
%% Cell type:code id: tags:
``` python
function sys=mdlGetTimeOfNextVarHit(t,x,u)
sys = [];
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlTerminate</code>
performs any end of simulation tasks.
%% Cell type:code id: tags:
``` python
function sys=mdlTerminate(t,x,u)
sys = [];
end
```
%% Cell type:markdown id: tags:
## <span style='color:Gray'>Beispiel #3 </span>
%% Cell type:code id: tags:
``` python
function [sys,x0,str,ts] = kalman3(t,x,u,flag,Ts,xo,ro,r1,r2)
switch flag,
% The following outlines the general structure of an S-function.
% Initialization:
case 0,
[sys,x0,str,ts]=mdlInitializeSizes(Ts,xo,ro);
% Derivatives:
case 1,
sys=mdlDerivatives(t,x,u);
% Update:
case 2,
sys=mdlUpdate(t,x,u,r1,r2);
% Outputs:
case 3,
sys=mdlOutputs(t,x,u);
% GetTimeOfNextVarHit
case 4,
sys=mdlGetTimeOfNextVarHit(t,x,u,Ts);
% Terminate:
case 9,
sys=mdlTerminate(t,x,u);
% Unexpected flags:
otherwise
error(['Unhandled flag = ',num2str(flag)]);
end
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlInitializeSizes</code>
returns the sizes, initial conditions, and sample times for the S-function.
%% Cell type:code id: tags:
``` python
sizes = simsizes;
```
%% Cell type:code id: tags:
``` python
function [sys,x0,str,ts]=mdlInitializeSizes(Ts,xo,ro)
% Call simsizes for a sizes structure, fill it in and convert it to a sizes array.
% Note that in this example, the values are hard coded. This is not a recommended
% practice as the characteristics of the block are typically defined by the S-function parameters.
sizes = simsizes;
sizes.NumContStates = 0;
sizes.NumDiscStates = 2;
sizes.NumOutputs = 1;
sizes.NumInputs = 2;
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
% initialize the initial conditions:
x0 = [xo ro];
% str is always an empty matrix:
str = [];
% initialize the array of sample times:
ts = [Ts 0];
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlDerivatives</code>
returns the derivatives for the continuous states.
%% Cell type:code id: tags:
``` python
function sys=mdlDerivatives(t,x,u)
sys = [];
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code> mdlUpdate</code>
handles discrete state updates, sample time hits, and major time step
requirements.
%% Cell type:code id: tags:
``` python
function sys=mdlUpdate(t,x,u,r1,r2)
sys(2) = r2*(r1+0.25*x(2))/(r2+r1+0.25*x(2));
K = sys(2)/(r2);
sys(1) = 0.5*x(1)+u(2)+K*(u(1)-(0.25*x(1)+u(2)));
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlOutputs</code>
returns the block outputs.
%% Cell type:code id: tags:
``` python
function sys=mdlOutputs(t,x,u)
sys(1) = x(1);
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code> mdlGetTimeOfNextVarHit</code>
returns the time of the next hit for this block. Note that the result is
absolute time. Note that this function is only used when you specify a
variable discrete-time sample time [-2 0] in the sample time array in
<code>mdlInitializeSizes</code>.
%% Cell type:code id: tags:
``` python
function sys=mdlGetTimeOfNextVarHit(t,x,u)
sys = [];
end
```
%% Cell type:markdown id: tags:
<div style="font-family: 'times'; font-size: 13pt; text-align: justify">
<code>mdlTerminate</code>
performs any end of simulation tasks.
%% Cell type:code id: tags:
``` python
function sys=mdlTerminate(t,x,u)
sys = [];
end
```
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment