Skip to content

Commit

Permalink
Added Better_Predictions_Than_Kalman.ipynb
Browse files Browse the repository at this point in the history
  • Loading branch information
@juangburgos
juangburgos committed May 2, 2024
1 parent 73f8bd6 commit 05f44e2
Showing 1 changed file with 162 additions and 0 deletions.
162 changes: 162 additions & 0 deletions docs/files/Better_Predictions_Than_Kalman.ipynb
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,162 @@
{
"metadata": {
"kernelspec": {
"name": "python",
"display_name": "Python (Pyodide)",
"language": "python"
},
"language_info": {
"codemirror_mode": {
"name": "python",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.8"
}
},
"nbformat_minor": 4,
"nbformat": 4,
"cells": [
{
"cell_type": "code",
"source": "import matplotlib_inline\nimport numpy as np\nimport scipy.signal as sig\nimport matplotlib.pyplot as plt\nimport math\nimport pyarma as pa\nfrom ipywidgets import interact",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
},
{
"cell_type": "code",
"source": "# function that accepts 2rd ord params and returns pyarma discrete state space matrices\ndef second_order_ss_model(k = 41, wn = 0.1, gi = 0.2, t_step = 1):\n # create state space model\n wn2 = pow(wn, 2)\n num = [k*wn2]\n den = [1, 2*gi*wn, wn2]\n (num, den) = sig.normalize(num, den)\n sys_ss_c = sig.tf2ss(num, den)\n \n # discrete state space model\n (Ad, Bd, Cd, Dd, _) = sig.cont2discrete(sys_ss_c, t_step)\n return pa.mat(Ad), pa.mat(Bd), pa.mat(Cd), pa.mat(Dd)\n\ndef sim_vectors(x_ini, t_step, t_len):\n # empty time, input, state, output vectors\n t = pa.regspace(0, t_step, t_len).t()\n u = pa.ones(pa.size(t))\n x = pa.zeros(x_ini.n_rows, t.n_cols)\n y = pa.zeros(1, t.n_cols)\n # set initial conditions\n x[:,0] = x_ini\n return t, u, x, y\n\ndef sim_plot(t, x_sys, y_sys, x_est = None, y_est = None, t_line = None):\n # measurement\n plt.subplot(3, 1, 1)\n plt.plot(t.t(), y_sys.t(), 'k', linewidth=1.5, label='y_sys (measurement)')\n if y_est is not None:\n plt.plot(t.t(), y_est.t(), 'r', alpha=0.75, linewidth=1, label='y_obs (estimate)')\n plt.legend(loc='lower right', fontsize=\"8\", shadow=True, framealpha=1)\n plt.ylim((0, 100))\n plt.grid(alpha=0.3)\n if t_line is not None:\n plt.axvline(x=t_line, color='k', linewidth=1.5, linestyle=\"--\")\n # state 1\n plt.subplot(3, 1, 2)\n plt.plot(t.t(), x_sys[0,:].t(), 'k', linewidth=1.5, label='x1_sys (state 1)')\n if x_est is not None:\n plt.plot(t.t(), x_est[0,:].t(), 'r', alpha=0.75, linewidth=1, label='x1_obs (state 1 estimate)')\n plt.legend(loc='lower right', fontsize=\"8\", shadow=True, framealpha=1)\n plt.ylim((-40, 60))\n plt.grid(alpha=0.3)\n if t_line is not None:\n plt.axvline(x=t_line, color='k', linewidth=1.5, linestyle=\"--\")\n # state 2\n plt.subplot(3, 1, 3)\n plt.plot(t.t(), x_sys[1,:].t(), 'k', linewidth=1.5, label='x2_sys (state 2)')\n if x_est is not None:\n plt.plot(t.t(), x_est[1,:].t(), 'r', alpha=0.75, linewidth=1, label='x2_obs (state 2 estimate)')\n plt.legend(loc='lower right', fontsize=\"8\", shadow=True, framealpha=1)\n plt.ylim((0, 200))\n plt.grid(alpha=0.3)\n if t_line is not None:\n plt.axvline(x=t_line, color='k', linewidth=1.5, linestyle=\"--\")\n plt.xlabel('t')\n plt.show()\n\ndef sim_second_order(\n k = 41, \n wn = 0.1, \n gi = 0.2, \n x1_sys_ini = 1, \n x2_sys_ini = 150\n):\n # time step, time length\n t_step = 1; t_len = 140\n # discrete system state space model\n (Ad_sys, Bd_sys, Cd_sys, Dd_sys) = second_order_ss_model(k, wn, gi, t_step) \n # time, input, system state and output vectors\n (t, u, x_sys, y_sys) = sim_vectors(pa.mat([x1_sys_ini, x2_sys_ini]).t(), t_step, t_len)\n \n # run simulation\n for k in range(0, u.n_cols):\n # system output equation\n y_sys[:,k] = Cd_sys * x_sys[:,k] + Dd_sys * u[:,k]\n # system state equation\n if k < u.n_cols - 1:\n x_sys[:,k+1] = Ad_sys * x_sys[:,k] + Bd_sys * u[:,k]\n\n # show simulation results\n sim_plot(t, x_sys, y_sys)",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
},
{
"cell_type": "code",
"source": "interact(\n sim_second_order, \n k = (1, 100, 10), \n wn = (0.1, 0.35, 0.05), \n gi = (0.1, 2, 0.1),\n x1_sys_ini = (0, 10, 1), \n x2_sys_ini = (100, 200, 5)\n)",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
},
{
"cell_type": "markdown",
"source": "## The Kalman Filter",
"metadata": {}
},
{
"cell_type": "code",
"source": "from scipy.linalg import solve_discrete_are as dare",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
},
{
"cell_type": "code",
"source": "def kalman_gain(Ad, Cd, q = 5.1, r = 5.1):\n Qd = q * np.identity(Ad.n_cols)\n Rd = r * np.identity(Cd.n_rows)\n Pd = pa.mat(dare(Ad.t(), Cd.t(), Qd, Rd))\n Kd = Pd * Cd.t() * pa.pinv(pa.eye(Cd.n_rows, Cd.n_rows) + Cd * Pd * Cd.t())\n return Kd\n\n# simulate kalman filter with given diagonal values for the weight matrices\ndef sim_kalman(q = 5.1, r = 5.1, enable = True):\n # fixed simulation parameters\n k = 41; wn = 0.1; gi = 0.2; t_step = 1; t_len = 140\n x1_sys_ini = 1; x2_sys_ini = 150\n # discrete system state space model\n (Ad_sys, Bd_sys, Cd_sys, Dd_sys) = second_order_ss_model(k, wn, gi, t_step) \n # time, input, system state and output vectors\n (t, u, x_sys, y_sys) = sim_vectors(pa.mat([x1_sys_ini, x2_sys_ini]).t(), t_step, t_len)\n\n # discrete estimated state space model\n # NOTE : here we assume we know the system perfectly\n Ad_est = Ad_sys; Bd_est = Bd_sys; Cd_est = Cd_sys; Dd_est = Dd_sys\n # compute kalman observer gain \n Kd = kalman_gain(Ad_est, Cd_est, q, r)\n # observer state and output vectors\n # NOTE : here we assume we do not know the system's initial conditions, initial guess is [0, 0]\n x1_est_ini = 0; x2_est_ini = 0\n (_, _, x_est, y_est) = sim_vectors(pa.mat([x1_est_ini, x2_est_ini]).t(), t_step, t_len)\n \n # run simulation\n for k in range(0, u.n_cols):\n # system output equation (measurement)\n y_sys[:,k] = Cd_sys * x_sys[:,k] + Dd_sys * u[:,k]\n # system state equation\n if k < u.n_cols - 1:\n x_sys[:,k+1] = Ad_sys * x_sys[:,k] + Bd_sys * u[:,k]\n \n # observer output equation\n y_est[:,k] = Cd_est * x_est[:,k] + Dd_est * u[:,k]\n # observer state equation\n if k < u.n_cols - 1:\n x_est[:,k+1] = Ad_est * x_est[:,k] + Bd_est * u[:,k]\n # correction factor\n if enable:\n y_err = y_sys[:,k] - y_est[:,k]\n x_est[:,k+1] = x_est[:,k+1] + Kd * y_err\n\n # show simulation results\n sim_plot(t, x_sys, y_sys, x_est, y_est)",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
},
{
"cell_type": "code",
"source": "interact(\n sim_kalman, \n q = (0, 10, 1), \n r = (0, 10, 1),\n enable = True\n)",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
},
{
"cell_type": "markdown",
"source": "## Disturbance and Noise",
"metadata": {}
},
{
"cell_type": "code",
"source": "# simulate kalman filter under different types of disturbance and noise\ndef sim_kalman_disturbance(dist_param, dist_factor = 1.0, dist_output = False, noise_output = False):\n # fixed simulation parameters\n q = 5.1; r = 5.1\n k = 41; wn = 0.1; gi = 0.2; t_step = 1; t_len = 140\n x1_sys_ini = 1; x2_sys_ini = 150\n # discrete system state space model\n # NOTE : here we add parametric disturbance\n (Ad_sys, Bd_sys, Cd_sys, Dd_sys) = second_order_ss_model(\n dist_factor * k if dist_param == 'k' else k, \n dist_factor * wn if dist_param == 'wn' else wn, \n dist_factor * gi if dist_param == 'gi' else gi, \n t_step\n )\n # time, input, system state and output vectors\n (t, u, x_sys, y_sys) = sim_vectors(pa.mat([x1_sys_ini, x2_sys_ini]).t(), t_step, t_len)\n # noise vector\n n = pa.mat(np.random.default_rng().random(u.n_cols))\n\n # discrete estimated state space model\n (Ad_est, Bd_est, Cd_est, Dd_est) = second_order_ss_model(k, wn, gi, t_step)\n # compute kalman observer gain \n Kd = kalman_gain(Ad_est, Cd_est, q, r)\n # observer state and output vectors\n # NOTE : here we assume we do not know the system's initial conditions, initial guess is [0, 0]\n x1_est_ini = 0; x2_est_ini = 0\n (_, _, x_est, y_est) = sim_vectors(pa.mat([x1_est_ini, x2_est_ini]).t(), t_step, t_len)\n \n # run simulation\n for k in range(0, u.n_cols):\n # system output equation (measurement)\n y_sys[:,k] = Cd_sys * x_sys[:,k] + Dd_sys * u[:,k]\n # additive disturbance at the output\n if dist_output:\n y_sys[:,k] = y_sys[:,k] + 0.4 * t[:,k]\n if noise_output:\n y_sys[:,k] = y_sys[:,k] + 8.0 * n[:,k]\n # system state equation\n if k < u.n_cols - 1:\n x_sys[:,k+1] = Ad_sys * x_sys[:,k] + Bd_sys * u[:,k]\n \n # observer output equation\n y_est[:,k] = Cd_est * x_est[:,k] + Dd_est * u[:,k]\n # observer state equation\n if k < u.n_cols - 1:\n x_est[:,k+1] = Ad_est * x_est[:,k] + Bd_est * u[:,k]\n # correction factor\n y_err = y_sys[:,k] - y_est[:,k]\n x_est[:,k+1] = x_est[:,k+1] + Kd * y_err\n\n # show simulation results\n sim_plot(t, x_sys, y_sys, x_est, y_est)",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
},
{
"cell_type": "code",
"source": "interact(\n sim_kalman_disturbance, \n dist_param = ['k','wn', 'gi'], \n dist_factor = (0.5, 2, 0.1),\n dist_output = False\n)",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
},
{
"cell_type": "markdown",
"source": "## Prediction",
"metadata": {}
},
{
"cell_type": "code",
"source": "# simulate kalman filter under different types of disturbance\ndef sim_kalman_prediction(dist_param, dist_factor = 1.0, dist_output = False, predict_from = 70):\n # fixed simulation parameters\n q = 5.1; r = 5.1\n k = 41; wn = 0.1; gi = 0.2; t_step = 1; t_len = 140\n x1_sys_ini = 1; x2_sys_ini = 150\n # discrete system state space model\n # NOTE : here we add parametric disturbance\n (Ad_sys, Bd_sys, Cd_sys, Dd_sys) = second_order_ss_model(\n dist_factor * k if dist_param == 'k' else k, \n dist_factor * wn if dist_param == 'wn' else wn, \n dist_factor * gi if dist_param == 'gi' else gi, \n t_step\n )\n # time, input, system state and output vectors\n (t, u, x_sys, y_sys) = sim_vectors(pa.mat([x1_sys_ini, x2_sys_ini]).t(), t_step, t_len)\n\n # discrete estimated state space model\n (Ad_est, Bd_est, Cd_est, Dd_est) = second_order_ss_model(k, wn, gi, t_step)\n # compute kalman observer gain \n Kd = kalman_gain(Ad_est, Cd_est, q, r)\n # observer state and output vectors\n # NOTE : here we assume we do not know the system's initial conditions, initial guess is [0, 0]\n x1_est_ini = 0; x2_est_ini = 0\n (_, _, x_est, y_est) = sim_vectors(pa.mat([x1_est_ini, x2_est_ini]).t(), t_step, t_len)\n \n # run simulation\n for k in range(0, u.n_cols):\n # system output equation (measurement)\n y_sys[:,k] = Cd_sys * x_sys[:,k] + Dd_sys * u[:,k]\n # additive disturbance at the output\n if dist_output:\n y_sys[:,k] = y_sys[:,k] + 0.4 * t[:,k]\n # system state equation\n if k < u.n_cols - 1:\n x_sys[:,k+1] = Ad_sys * x_sys[:,k] + Bd_sys * u[:,k]\n \n # observer output equation\n y_est[:,k] = Cd_est * x_est[:,k] + Dd_est * u[:,k]\n # observer state equation\n if k < u.n_cols - 1:\n x_est[:,k+1] = Ad_est * x_est[:,k] + Bd_est * u[:,k]\n # correction factor\n # NOTE : at time equals t >= predict_from we stop taking measurements into consideration\n if t[0,k] < predict_from:\n y_err = y_sys[:,k] - y_est[:,k]\n x_est[:,k+1] = x_est[:,k+1] + Kd * y_err\n\n # show simulation results\n sim_plot(t, x_sys, y_sys, x_est, y_est, predict_from)",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
},
{
"cell_type": "code",
"source": "interact(\n sim_kalman_prediction, \n dist_param = ['k','wn', 'gi'], \n dist_factor = (0.2, 2, 0.1),\n dist_output = False,\n predict_from = (0, 140, 10)\n)",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
},
{
"cell_type": "markdown",
"source": "## Better Alternative",
"metadata": {}
},
{
"cell_type": "code",
"source": "# accepts 2rd ord params and desired closed loop bandwidth (tao)\n# and returns the filter's pyarma discrete state space matrices\ndef filter_ss_model(k = 41, wn = 0.1, gi = 0.2, tao = 0.05, t_step = 1):\n # create state space model\n wp = 100\n wn2 = pow(wn, 2)\n wpt2 = wp*pow(tao, 2)\n num = [wpt2, 2*gi*wn*wpt2, wn2*wpt2]\n den = [k*wn2, k*wn2*(wp + 1)*tao, 0]\n (num, den) = sig.normalize(num, den)\n filt_ss_c = sig.tf2ss(num, den)\n # discrete state space model\n (Ad, Bd, Cd, Dd, _) = sig.cont2discrete(filt_ss_c, t_step)\n # fix input matrix\n (_, Bd_est, _, _) = second_order_ss_model(k, wn, gi, t_step)\n return pa.mat(Ad), pa.mat(Bd), Bd_est*pa.mat(Cd), Bd_est*pa.mat(Dd)\n",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
},
{
"cell_type": "code",
"source": "# simulate alternative filter under different types of disturbance\ndef sim_filter_prediction(dist_param, dist_factor = 1.0, dist_output = False, predict_from = 70, tao = 0.04):\n # fixed simulation parameters\n k = 41; wn = 0.1; gi = 0.2; t_step = 1; t_len = 140\n x1_sys_ini = 1; x2_sys_ini = 150\n # discrete system state space model\n # NOTE : here we add parametric disturbance\n (Ad_sys, Bd_sys, Cd_sys, Dd_sys) = second_order_ss_model(\n dist_factor * k if dist_param == 'k' else k, \n dist_factor * wn if dist_param == 'wn' else wn, \n dist_factor * gi if dist_param == 'gi' else gi, \n t_step\n )\n # time, input, system state and output vectors\n (t, u, x_sys, y_sys) = sim_vectors(pa.mat([x1_sys_ini, x2_sys_ini]).t(), t_step, t_len)\n\n # discrete estimated state space model\n (Ad_est, Bd_est, Cd_est, Dd_est) = second_order_ss_model(k, wn, gi, t_step)\n # compute alternative filter\n (Ad_flt, Bd_flt, Cd_flt, Dd_flt) = filter_ss_model(k, wn, gi, tao, t_step)\n # observer state and output vectors\n # NOTE : here we assume we do not know the system's initial conditions, initial guess is [0, 0]\n x1_est_ini = 0; x2_est_ini = 0\n (_, _, x_est, y_est) = sim_vectors(pa.mat([x1_est_ini, x2_est_ini]).t(), t_step, t_len)\n # filter state vector\n (_, _, x_flt, _) = sim_vectors(pa.zeros(Ad_flt.n_rows,1), t_step, t_len)\n \n # run simulation\n for k in range(0, u.n_cols):\n # system output equation (measurement)\n y_sys[:,k] = Cd_sys * x_sys[:,k] + Dd_sys * u[:,k]\n # additive disturbance at the output\n if dist_output:\n y_sys[:,k] = y_sys[:,k] + 0.4 * t[:,k]\n # system state equation\n if k < u.n_cols - 1:\n x_sys[:,k+1] = Ad_sys * x_sys[:,k] + Bd_sys * u[:,k]\n \n # filter output equation\n y_est[:,k] = Cd_est * x_est[:,k] + Dd_est * u[:,k]\n # filter state equation\n if k < u.n_cols - 1:\n x_flt[:,k+1] = Ad_flt * x_flt[:,k]\n x_est[:,k+1] = Ad_est * x_est[:,k] + Bd_est * u[:,k] + Cd_flt * x_flt[:,k]\n # correction factor\n # NOTE : at time equals t >= predict_from we stop taking measurements into consideration\n if t[0,k] < predict_from:\n y_err = y_sys[:,k] - y_est[:,k]\n x_flt[:,k+1] = x_flt[:,k+1] + Bd_flt * y_err\n x_est[:,k+1] = x_est[:,k+1] + Dd_flt * y_err\n\n # show simulation results\n sim_plot(t, x_sys, y_sys, x_est, y_est, predict_from)",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
},
{
"cell_type": "code",
"source": "interact(\n sim_filter_prediction, \n dist_param = ['k','wn', 'gi'], \n dist_factor = (0.2, 2, 0.1),\n dist_output = False,\n predict_from = (0, 140, 10),\n tao = (0.001, 0.15, 0.001)\n)",
"metadata": {
"trusted": true
},
"outputs": [],
"execution_count": null
}
]
}

0 comments on commit 05f44e2

Please sign in to comment.