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