"# <span style='color:OrangeRed'>V10 KALMAN FILTER - TEIL 2</span>"
]
},
{
"cell_type": "markdown",
"id": "0eca61e4-d40e-4255-b7c8-f866af94c1cd",
"metadata": {},
"source": [
"Das folgende lineare, zeitdiskrete sei gegeben:"
]
},
{
"cell_type": "code",
"execution_count": 1,
"id": "efe2a8c2-841a-4f63-813c-4c137247929f",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"As = 1\n",
"Bs = 1\n",
"Cs = 0.50000\n"
]
}
],
"source": [
"As = 1\n",
"Bs = 1\n",
"Cs = 0.5"
]
},
{
"cell_type": "markdown",
"id": "4088bf0b-5578-4e2d-8553-a40b8f92d4a4",
"metadata": {},
"source": [
"Der Anfangszustand sei zweifelsfrei gegeben:"
]
},
{
"cell_type": "code",
"execution_count": 2,
"id": "fc64f0c8-a86d-4c90-b938-4b70ff1b2cea",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"xo = 1\n",
"xov = 0\n"
]
}
],
"source": [
"xo = 1\n",
"xov = 0"
]
},
{
"cell_type": "markdown",
"id": "09876fc3-650d-4f38-a936-b82de673812f",
"metadata": {},
"source": [
"Anders als in der Standardform, wird das Prozessrauschen durch ein Störmodell beschrieben:"
]
},
{
"cell_type": "code",
"execution_count": 4,
"id": "c699f9e3-7886-4f2a-ba27-f0c6a40cedf0",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Ar = 1\n",
"Br = 1\n"
]
}
],
"source": [
"Ar = 1\n",
"Br = 1"
]
},
{
"cell_type": "markdown",
"id": "a05183dd-1ff7-477a-bea7-177bdac6d3be",
"metadata": {},
"source": [
"Eingang für das Störmodell ist ein mittelwertfreies Signal mit der Gauß'schen Verteilung und Varianz: "
]
},
{
"cell_type": "code",
"execution_count": 5,
"id": "157ab316-4e77-4df1-a3de-3f6c09f81618",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"rk = 0.50000\n"
]
}
],
"source": [
"rk = 0.5"
]
},
{
"cell_type": "markdown",
"id": "91c85755-9ef5-40d9-8331-43b9dcac0aac",
"metadata": {},
"source": [
"Das Messrauschen ist ebenso Gauß-verteilt mit Varianz:"
]
},
{
"cell_type": "code",
"execution_count": 6,
"id": "667a52bf-771b-40b8-a598-67f3bcb1b86d",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"sk = 0.50000\n"
]
}
],
"source": [
"sk = 0.5"
]
},
{
"cell_type": "markdown",
"id": "0f8c69f5-d96b-4bae-85e7-cafdce802b5d",
"metadata": {},
"source": [
"Passen Sie die Zusandsraumdarstellung so an, dass die bekannten Formeln des Kalman-Filter angewendet werden können. Fasses Sie zu diesem Zweck das Störmodell und das zeitdiskrete System zu einem neuen Gesamtsystem zusammen."
]
},
{
"cell_type": "code",
"execution_count": 7,
"id": "92391d85-4a55-4191-b9db-abd84a6b9be0",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Ag =\n",
"\n",
" 1 1\n",
" 0 1\n",
"\n",
"Bg =\n",
"\n",
" 1\n",
" 0\n",
"\n",
"Bd =\n",
"\n",
" 0\n",
" 1\n",
"\n",
"Cg =\n",
"\n",
" 0.50000 0.00000\n",
"\n"
]
}
],
"source": [
"Ag = [As 1;0 Ar]\n",
"Bg = [Bs;0]\n",
"Bd = [0; Br]\n",
"Cg = [Cs 0]\n"
]
},
{
"cell_type": "markdown",
"id": "aab57f38-99e5-4a6f-8ae7-e6c907a40762",
"metadata": {},
"source": [
"Berechnen Sie die Varianz des Messrauschens und des Prozessrauschens. Berechnen Sie zusätlich die Kalman Filter Anfangswerte."
]
},
{
"cell_type": "code",
"execution_count": 8,
"id": "c17a8cd9-f92b-4b63-8732-55d484cff5c2",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Rk = 0.50000\n",
"Qk =\n",
"\n",
" 0.00000 0.00000\n",
" 0.00000 0.50000\n",
"\n",
"Sigmao =\n",
"\n",
" 0 0\n",
" 0 0\n",
"\n"
]
}
],
"source": [
"Rk = rk\n",
"Qk = Bd*rk*Bd'\n",
"Sigmao=zeros(2,2)"
]
},
{
"cell_type": "markdown",
"id": "118da55d-dd12-414d-99a7-cccfa1241412",
"metadata": {},
"source": [
"Es gilt uk=-1. Führen Sie nun einen weiteren Prädiktionsschritt durch und bestimmen Sie die Schätzung des Zustandsvektors sowie die zugehörige Kovarianzmatrix."
]
},
{
"cell_type": "code",
"execution_count": 9,
"id": "e6eec8c3-8245-472e-acae-f06e707b6d4e",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"eta0 =\n",
"\n",
" 1\n",
" 0\n",
"\n",
"uk = -1\n",
"y1 = 10\n",
"eta1m =\n",
"\n",
" 0\n",
" 0\n",
"\n",
"Sigma1m =\n",
"\n",
" 0.00000 0.00000\n",
" 0.00000 0.50000\n",
"\n",
"K1 =\n",
"\n",
" 0\n",
" 0\n",
"\n",
"eta1 =\n",
"\n",
" 0\n",
" 0\n",
"\n",
"Sigma1 =\n",
"\n",
" 0.00000 0.00000\n",
" 0.00000 0.50000\n",
"\n"
]
}
],
"source": [
"eta0 = [xo; 0]\n",
"uk = -1\n",
"y1 = 10 \n",
"eta1m = Ag*eta0+Bg*uk\n",
"Sigma1m = Ag*Sigmao*Ag'+Qk\n",
"K1 = Sigma1m*Cg'*(Cg*Sigma1m*Cg'+Rk)\n",
"eta1 = eta1m+K1*(y1-Cg*eta1m)\n",
"Sigma1 = (eye(2)-K1*Cg)*Sigma1m"
]
},
{
"cell_type": "markdown",
"id": "a5adb980-c7e1-45c8-bb0f-d2bb5c9e1c8c",
"metadata": {},
"source": [
"Es gilt weiterhin uk=-1. Führen Sie nun einen weiteren Prädiktionsschritt durch und bestimmen Sie die Schätzung des Zustandsvektors sowie die zugehörige Kovarianzmatrix."
]
},
{
"cell_type": "code",
"execution_count": 10,
"id": "fc517de3-3041-4aa5-9377-80416d82bd74",
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"uk2 = -1\n",
"eta2 =\n",
"\n",
" -1\n",
" 0\n",
"\n",
"Sigma2m =\n",
"\n",
" 0.50000 0.50000\n",
" 0.50000 1.00000\n",
"\n"
]
}
],
"source": [
"uk2 = -1\n",
"eta2 = Ag*eta1+Bg*uk2\n",
"Sigma2m = Ag*Sigma1*Ag'+Qk\n"
]
},
{
"cell_type": "markdown",
"id": "825ff5a3-d5aa-4a1d-b831-0cbd75f1ef8e",
"metadata": {},
"source": [
"Das Ergebnis eines weiteren Korrekturschritts ist nun abhängig von der Messung y2, da\n",
"die aktuelle Zustandsschätzung nur durch das Prozessrauschen gestört ist und somit\n",
"mittels Messung die Unsicherheit reduziert werden kann."
Passen Sie die Zusandsraumdarstellung so an, dass die bekannten Formeln des Kalman-Filter angewendet werden können. Fasses Sie zu diesem Zweck das Störmodell und das zeitdiskrete System zu einem neuen Gesamtsystem zusammen.
Es gilt uk=-1. Führen Sie nun einen weiteren Prädiktionsschritt durch und bestimmen Sie die Schätzung des Zustandsvektors sowie die zugehörige Kovarianzmatrix.
Es gilt weiterhin uk=-1. Führen Sie nun einen weiteren Prädiktionsschritt durch und bestimmen Sie die Schätzung des Zustandsvektors sowie die zugehörige Kovarianzmatrix.
