{ "cells": [ { "cell_type": "markdown", "id": "baba5fab", "metadata": {}, "source": [ "# Moving Horizon Estimation\n", "\n", "Richard M. Murray, 24 Feb 2023\n", "\n", "In this notebook we illustrate the implementation of moving horizon estimation (MHE)" ] }, { "cell_type": "code", "execution_count": null, "id": "36715c5f", "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "import scipy as sp\n", "import matplotlib.pyplot as plt\n", "import control as ct\n", "\n", "import control.optimal as opt\n", "import control.flatsys as fs" ] }, { "cell_type": "code", "execution_count": null, "id": "1a4e4084", "metadata": {}, "outputs": [], "source": [ "# Import the new MHE routines (to be moved to python-control)\n", "import ctrlutil as opt_" ] }, { "cell_type": "markdown", "id": "d72a155b", "metadata": {}, "source": [ "## System Description\n", "\n", "The dynamics of the system\n", "with disturbances on the $x$ and $y$ variables is given by\n", "\n", "\n", " \\begin{aligned}\n", " m \\ddot x &= F_1 \\cos\\theta - F_2 \\sin\\theta - c \\dot x + d_x, \\\\\n", " m \\ddot y &= F_1 \\sin\\theta + F_2 \\cos\\theta - c \\dot y - m g + d_y, \\\\\n", " J \\ddot \\theta &= r F_1.\n", " \\end{aligned}\n", "\n", "\n", "The measured values of the system are the position and orientation,\n", "with added noise $n_x$, $n_y$, and $n_\\theta$:\n", "\n", "$$\n", " \\vec y = \\begin{bmatrix} x \\\\ y \\\\ \\theta \\end{bmatrix} + \n", " \\begin{bmatrix} n_x \\\\ n_y \\\\ n_z \\end{bmatrix}.\n", "$$" ] }, { "cell_type": "code", "execution_count": null, "id": "08919988", "metadata": {}, "outputs": [], "source": [ "# pvtol = nominal system (no disturbances or noise)\n", "# noisy_pvtol = pvtol w/ process disturbances and sensor noise\n", "from pvtol import pvtol, pvtol_noisy, plot_results\n", "import pvtol as pvt\n", "\n", "# Find the equiblirum point corresponding to the origin\n", "xe, ue = ct.find_eqpt(\n", " pvtol, np.zeros(pvtol.nstates),\n", " np.zeros(pvtol.ninputs), [0, 0, 0, 0, 0, 0],\n", " iu=range(2, pvtol.ninputs), iy=[0, 1])\n", "\n", "# Initial condition = 2 meters right, 1 meter up\n", "x0, u0 = ct.find_eqpt(\n", " pvtol, np.zeros(pvtol.nstates),\n", " np.zeros(pvtol.ninputs), np.array([2, 1, 0, 0, 0, 0]),\n", " iu=range(2, pvtol.ninputs), iy=[0, 1])\n", "\n", "# Extract the linearization for use in LQR design\n", "pvtol_lin = pvtol.linearize(xe, ue)\n", "A, B = pvtol_lin.A, pvtol_lin.B\n", "\n", "print(pvtol, \"\\n\")\n", "print(pvtol_noisy)" ] }, { "cell_type": "markdown", "id": "5771ab93", "metadata": {}, "source": [ "### Control Design" ] }, { "cell_type": "code", "execution_count": null, "id": "d2e88938", "metadata": {}, "outputs": [], "source": [ "#\n", "# LQR design w/ physically motivated weighting\n", "#\n", "# Shoot for 10 cm error in x, 10 cm error in y. Try to keep the angle\n", "# less than 5 degrees in making the adjustments. Penalize side forces\n", "# due to loss in efficiency.\n", "#\n", "\n", "Qx = np.diag([100, 10, (180/np.pi) / 5, 0, 0, 0])\n", "Qu = np.diag([10, 1])\n", "K, _, _ = ct.lqr(A, B, Qx, Qu)\n", "\n", "# Compute the full state feedback solution\n", "lqr_ctrl, _ = ct.create_statefbk_iosystem(pvtol, K)\n", "\n", "# Define the closed loop system that will be used to generate trajectories\n", "lqr_clsys = ct.interconnect(\n", " [pvtol_noisy, lqr_ctrl],\n", " inplist = lqr_ctrl.input_labels[0:pvtol.ninputs + pvtol.nstates] + \\\n", " pvtol_noisy.input_labels[pvtol.ninputs:],\n", " inputs = lqr_ctrl.input_labels[0:pvtol.ninputs + pvtol.nstates] + \\\n", " pvtol_noisy.input_labels[pvtol.ninputs:],\n", " outlist = pvtol.output_labels + lqr_ctrl.output_labels,\n", " outputs = pvtol.output_labels + lqr_ctrl.output_labels\n", ")\n", "print(lqr_clsys)" ] }, { "cell_type": "code", "execution_count": null, "id": "78853391", "metadata": {}, "outputs": [], "source": [ "# Disturbance and noise intensities\n", "Qv = np.diag([1e-2, 1e-2])\n", "Qw = np.array([[1e-4, 0, 1e-5], [0, 1e-4, 1e-5], [1e-5, 1e-5, 1e-4]])\n", "\n", "# Initial state covariance\n", "P0 = np.eye(pvtol.nstates)" ] }, { "cell_type": "code", "execution_count": null, "id": "c590fd88", "metadata": {}, "outputs": [], "source": [ "# Create the time vector for the simulation\n", "Tf = 6\n", "timepts = np.linspace(0, Tf, 20)\n", "\n", "# Create representative process disturbance and sensor noise vectors\n", "# np.random.seed(117) # avoid figures changing from run to run\n", "V = ct.white_noise(timepts, Qv)\n", "# V = np.clip(V0, -0.1, 0.1) # Hold for later\n", "W = ct.white_noise(timepts, Qw)\n", "# plt.plot(timepts, V0[0], 'b--', label=\"V[0]\")\n", "plt.plot(timepts, V[0], label=\"V[0]\")\n", "plt.plot(timepts, W[0], label=\"W[0]\")\n", "plt.legend();" ] }, { "cell_type": "code", "execution_count": null, "id": "c35fd695", "metadata": {}, "outputs": [], "source": [ "# Desired trajectory\n", "xd, ud = xe, ue\n", "# xd = np.vstack([\n", "# np.sin(2 * np.pi * timepts / timepts[-1]), \n", "# np.zeros((5, timepts.size))])\n", "# ud = np.outer(ue, np.ones_like(timepts))\n", "\n", "# Run a simulation with full state feedback (no noise) to generate a trajectory\n", "uvec = [xd, ud, V, W*0]\n", "lqr_resp = ct.input_output_response(lqr_clsys, timepts, uvec, x0)\n", "U = lqr_resp.outputs[6:8] # controller input signals\n", "Y = lqr_resp.outputs[0:3] + W # noisy output signals (noise in pvtol_noisy)\n", "\n", "# Compare to the no noise case\n", "uvec = [xd, ud, V*0, W*0]\n", "lqr0_resp = ct.input_output_response(lqr_clsys, timepts, uvec, x0)\n", "lqr0_fine = ct.input_output_response(lqr_clsys, timepts, uvec, x0, \n", " t_eval=np.linspace(timepts[0], timepts[-1], 100))\n", "U0 = lqr0_resp.outputs[6:8]\n", "Y0 = lqr0_resp.outputs[0:3]\n", "\n", "# Compare the results\n", "# plt.plot(Y0[0], Y0[1], 'k--', linewidth=2, label=\"No disturbances\")\n", "plt.plot(lqr0_fine.states[0], lqr0_fine.states[1], 'r-', label=\"Actual\")\n", "plt.plot(Y[0], Y[1], 'b-', label=\"Noisy\")\n", "\n", "plt.xlabel('$x$ [m]')\n", "plt.ylabel('$y$ [m]')\n", "plt.axis('equal')\n", "plt.legend(frameon=False)\n", "\n", "plt.figure()\n", "plot_results(timepts, lqr_resp.states, lqr_resp.outputs[6:8]);" ] }, { "cell_type": "code", "execution_count": null, "id": "a7f1dec6", "metadata": {}, "outputs": [], "source": [ "# Utility functions for making plots\n", "def plot_state_comparison(\n", " timepts, est_states, act_states=None, estimated_label='$\\\\hat x_{i}$', actual_label='$x_{i}$',\n", " start=0):\n", " for i in range(sys.nstates):\n", " plt.subplot(2, 3, i+1)\n", " if act_states is not None:\n", " plt.plot(timepts[start:], act_states[i, start:], 'r--', \n", " label=actual_label.format(i=i))\n", " plt.plot(timepts[start:], est_states[i, start:], 'b', \n", " label=estimated_label.format(i=i))\n", " plt.legend()\n", " plt.tight_layout()\n", " \n", "# Define a function to plot out all of the relevant signals\n", "def plot_estimator_response(timepts, estimated, U, V, Y, W, start=0):\n", " # Plot the input signal and disturbance\n", " for i in [0, 1]:\n", " # Input signal (the same across all)\n", " plt.subplot(4, 3, i+1)\n", " plt.plot(timepts[start:], U[i, start:], 'k')\n", " plt.ylabel(f'U[{i}]')\n", "\n", " # Plot the estimated disturbance signal\n", " plt.subplot(4, 3, 4+i)\n", " plt.plot(timepts[start:], estimated.inputs[i, start:], 'b-', label=\"est\")\n", " plt.plot(timepts[start:], V[i, start:], 'k', label=\"actual\")\n", " plt.ylabel(f'V[{i}]')\n", "\n", " plt.subplot(4, 3, 6)\n", " plt.plot(0, 0, 'b', label=\"estimated\")\n", " plt.plot(0, 0, 'k', label=\"actual\")\n", " plt.plot(0, 0, 'r', label=\"measured\")\n", " plt.legend(frameon=False)\n", " plt.grid(False)\n", " plt.axis('off')\n", " \n", " # Plot the output (measured and estimated) \n", " for i in [0, 1, 2]:\n", " plt.subplot(4, 3, 7+i)\n", " plt.plot(timepts[start:], Y[i, start:], 'r', label=\"measured\")\n", " plt.plot(timepts[start:], estimated.states[i, start:], 'b', label=\"measured\")\n", " plt.plot(timepts[start:], Y[i, start:] - W[i, start:], 'k', label=\"actual\")\n", " plt.ylabel(f'Y[{i}]')\n", " \n", " for i in [0, 1, 2]:\n", " plt.subplot(4, 3, 10+i)\n", " plt.plot(timepts[start:], estimated.outputs[i, start:], 'b', label=\"estimated\")\n", " plt.plot(timepts[start:], W[i, start:], 'k', label=\"actual\")\n", " plt.ylabel(f'W[{i}]')\n", " plt.xlabel('Time [s]')\n", "\n", " plt.tight_layout()" ] }, { "cell_type": "markdown", "id": "73dd9be3", "metadata": {}, "source": [ "## State Estimation" ] }, { "cell_type": "code", "execution_count": null, "id": "5a1f32da", "metadata": {}, "outputs": [], "source": [ "# Create a new system with only x, y, theta as outputs\n", "# TODO: add this to pvtol.py?\n", "sys = ct.NonlinearIOSystem(\n", " pvt._noisy_update, lambda t, x, u, params: x[0:3], name=\"pvtol_noisy\",\n", " states = [f'x{i}' for i in range(6)],\n", " inputs = ['F1', 'F2'] + ['Dx', 'Dy'],\n", " outputs = ['x', 'y', 'theta']\n", ")" ] }, { "cell_type": "code", "execution_count": null, "id": "3a0679f4", "metadata": {}, "outputs": [], "source": [ "# Standard Kalman filter\n", "linsys = sys.linearize(xe, [ue, V[:, 0] * 0])\n", "# print(linsys)\n", "B = linsys.B[:, 0:2]\n", "G = linsys.B[:, 2:4]\n", "linsys = ct.ss(\n", " linsys.A, B, linsys.C, 0,\n", " states=sys.state_labels, inputs=sys.input_labels[0:2], outputs=sys.output_labels)\n", "# print(linsys)\n", "\n", "estim = ct.create_estimator_iosystem(linsys, Qv, Qw, G=G, P0=P0)\n", "print(estim)\n", "print(f'{xe=}, {P0=}')\n", "\n", "kf_resp = ct.input_output_response(\n", " estim, timepts, [Y, U], X0 = [xe, P0.reshape(-1)])\n", "plot_state_comparison(timepts, kf_resp.outputs, lqr_resp.states)" ] }, { "cell_type": "markdown", "id": "654dde1b", "metadata": {}, "source": [ "### Extended Kalman filter" ] }, { "cell_type": "code", "execution_count": null, "id": "1f83a335", "metadata": {}, "outputs": [], "source": [ "# Define the disturbance input and measured output matrices\n", "F = np.array([[0, 0], [0, 0], [0, 0], [1/pvtol.params['m'], 0], [0, 1/pvtol.params['m']], [0, 0]])\n", "C = np.eye(3, 6)\n", "\n", "Qwinv = np.linalg.inv(Qw)\n", "\n", "# Estimator update law\n", "def estimator_update(t, x, u, params):\n", " # Extract the states of the estimator\n", " xhat = x[0:pvtol.nstates]\n", " P = x[pvtol.nstates:].reshape(pvtol.nstates, pvtol.nstates)\n", "\n", " # Extract the inputs to the estimator\n", " y = u[0:3] # just grab the first three outputs\n", " u = u[6:8] # get the inputs that were applied as well\n", "\n", " # Compute the linearization at the current state\n", " A = pvtol.A(xhat, u) # A matrix depends on current state\n", " # A = pvtol.A(xe, ue) # Fixed A matrix (for testing/comparison)\n", " \n", " # Compute the optimal \"gain\n", " L = P @ C.T @ Qwinv\n", "\n", " # Update the state estimate\n", " xhatdot = pvtol.updfcn(t, xhat, u, params) - L @ (C @ xhat - y)\n", "\n", " # Update the covariance\n", " Pdot = A @ P + P @ A.T - P @ C.T @ Qwinv @ C @ P + F @ Qv @ F.T\n", "\n", " # Return the derivative\n", " return np.hstack([xhatdot, Pdot.reshape(-1)])\n", "\n", "def estimator_output(t, x, u, params):\n", " # Return the estimator states\n", " return x[0:pvtol.nstates]\n", "\n", "ekf = ct.NonlinearIOSystem(\n", " estimator_update, estimator_output,\n", " states=pvtol.nstates + pvtol.nstates**2,\n", " inputs= pvtol_noisy.output_labels \\\n", " + pvtol_noisy.input_labels[0:pvtol.ninputs],\n", " outputs=[f'xh{i}' for i in range(pvtol.nstates)]\n", ")\n", "print(ekf)" ] }, { "cell_type": "code", "execution_count": null, "id": "a4caf69b", "metadata": {}, "outputs": [], "source": [ "ekf_resp = ct.input_output_response(\n", " ekf, timepts, [lqr_resp.states, lqr_resp.outputs[6:8]],\n", " X0=[xe, P0.reshape(-1)])\n", "plot_state_comparison(timepts, ekf_resp.outputs, lqr_resp.states)" ] }, { "cell_type": "code", "execution_count": null, "id": "1074908c", "metadata": {}, "outputs": [], "source": [ "# Define the optimal estimation problem\n", "traj_cost = opt_.gaussian_likelihood_cost(sys, Qv, Qw)\n", "init_cost = lambda xhat, x: (xhat - x) @ P0 @ (xhat - x)\n", "oep = opt_.OptimalEstimationProblem(\n", " sys, timepts, traj_cost, terminal_cost=init_cost)\n", "\n", "# Compute the estimate from the noisy signals\n", "est = oep.compute_estimate(Y, U, X0=lqr_resp.states[:, 0])\n", "plot_state_comparison(timepts, est.states, lqr_resp.states)" ] }, { "cell_type": "code", "execution_count": null, "id": "0c6981b9", "metadata": {}, "outputs": [], "source": [ "# Plot the response of the estimator\n", "plot_estimator_response(timepts, est, U, V, Y, W)" ] }, { "cell_type": "code", "execution_count": null, "id": "25b8aa85", "metadata": {}, "outputs": [], "source": [ "# Noise free and disturbance free => estimation should be near perfect\n", "noisefree_cost = opt_.gaussian_likelihood_cost(sys, Qv, Qw*1e-6)\n", "oep0 = opt_.OptimalEstimationProblem(\n", " sys, timepts, noisefree_cost, terminal_cost=init_cost)\n", "est0 = oep0.compute_estimate(Y0, U0, X0=lqr0_resp.states[:, 0],\n", " initial_guess=(lqr0_resp.states, V * 0))\n", "plot_state_comparison(\n", " timepts, est0.states, lqr0_resp.states, estimated_label='$\\\\bar x_{i}$')" ] }, { "cell_type": "code", "execution_count": null, "id": "7a76821f", "metadata": {}, "outputs": [], "source": [ "plot_estimator_response(timepts, est0, U0, V*0, Y0, W*0)" ] }, { "cell_type": "markdown", "id": "6b9031cf", "metadata": {}, "source": [ "### Bounded disturbances\n", "\n", "Another thing that the MHE can handled is input distributions that are bounded. We implement that here by carrying out the optimal estimation problem with constraints." ] }, { "cell_type": "code", "execution_count": null, "id": "93482470", "metadata": {}, "outputs": [], "source": [ "V_clipped = np.clip(V, -0.05, 0.05) \n", "\n", "plt.plot(timepts, V[0], label=\"V[0]\")\n", "plt.plot(timepts, V_clipped[0], label=\"V[0] clipped\")\n", "plt.plot(timepts, W[0], label=\"W[0]\")\n", "plt.legend();" ] }, { "cell_type": "code", "execution_count": null, "id": "56e186f1", "metadata": {}, "outputs": [], "source": [ "uvec = [xe, ue, V_clipped, W]\n", "clipped_resp = ct.input_output_response(lqr_clsys, timepts, uvec, x0)\n", "U_clipped = clipped_resp.outputs[6:8] # controller input signals\n", "Y_clipped = clipped_resp.outputs[0:3] + W # noisy output signals\n", "\n", "traj_constraint = opt_.add_disturbance_range_constraint(\n", " sys, [-0.05, -0.05], [0.05, 0.05])\n", "oep_clipped = opt_.OptimalEstimationProblem(\n", " sys, timepts, traj_cost, terminal_cost=init_cost,\n", " trajectory_constraints=traj_constraint)\n", "\n", "est_clipped = oep_clipped.compute_estimate(\n", " Y_clipped, U_clipped, X0=lqr0_resp.states[:, 0])\n", "plot_state_comparison(timepts, est_clipped.states, lqr_resp.states)\n", "plt.suptitle(\"MHE with constraints\")\n", "plt.tight_layout()\n", "\n", "plt.figure()\n", "ekf_unclipped = ct.input_output_response(\n", " ekf, timepts, [clipped_resp.states, clipped_resp.outputs[6:8]],\n", " X0=[xe, P0.reshape(-1)])\n", "\n", "plot_state_comparison(timepts, ekf_unclipped.outputs, lqr_resp.states)\n", "plt.suptitle(\"EKF w/out constraints\")\n", "plt.tight_layout()" ] }, { "cell_type": "code", "execution_count": null, "id": "108c341a", "metadata": {}, "outputs": [], "source": [ "plot_estimator_response(timepts, est_clipped, U, V_clipped, Y, W)" ] }, { "cell_type": "markdown", "id": "430117ce", "metadata": {}, "source": [ "## Moving Horizon Estimation (MHE)\n", "\n", "We can now move to implementation of a moving horizon estimator, using our fixed horizon optimal estimator." ] }, { "cell_type": "code", "execution_count": null, "id": "121d67ba", "metadata": {}, "outputs": [], "source": [ "# Use a shorter horizon\n", "mhe_timepts = timepts[0:5]\n", "oep = opt_.OptimalEstimationProblem(\n", " sys, mhe_timepts, traj_cost, terminal_cost=init_cost)\n", "\n", "try:\n", " mhe = oep.create_mhe_iosystem(2)\n", " \n", " est_mhe = ct.input_output_response(\n", " mhe, timepts, [Y, U], X0=resp.states[:, 0], \n", " params={'verbose': True}\n", " )\n", " plot_state_comparison(timepts, est_mhe.states, lqr_resp.states)\n", "except:\n", " print(\"MHE for continuous time systems not implemented\")" ] }, { "cell_type": "code", "execution_count": null, "id": "1914ad96", "metadata": {}, "outputs": [], "source": [ "# Create discrete time version of PVTOL\n", "Ts = 0.1\n", "print(f\"Sample time: {Ts=}\")\n", "dsys = ct.NonlinearIOSystem(\n", " lambda t, x, u, params: x + Ts * sys.updfcn(t, x, u, params),\n", " sys.outfcn, dt=Ts, states=sys.state_labels,\n", " inputs=sys.input_labels, outputs=sys.output_labels,\n", ")\n", "print(dsys)" ] }, { "cell_type": "code", "execution_count": null, "id": "11162130", "metadata": {}, "outputs": [], "source": [ "# Create a new list of time points\n", "timepts = np.arange(0, Tf, Ts)\n", "\n", "# Create representative process disturbance and sensor noise vectors\n", "# np.random.seed(117) # avoid figures changing from run to run\n", "V = ct.white_noise(timepts, Qv)\n", "# V = np.clip(V0, -0.1, 0.1) # Hold for later\n", "W = ct.white_noise(timepts, Qw, dt=Ts)\n", "# plt.plot(timepts, V0[0], 'b--', label=\"V[0]\")\n", "plt.plot(timepts, V[0], label=\"V[0]\")\n", "plt.plot(timepts, W[0], label=\"W[0]\")\n", "plt.legend();" ] }, { "cell_type": "code", "execution_count": null, "id": "c8a6a693", "metadata": {}, "outputs": [], "source": [ "# Generate a new trajectory over the longer time vector\n", "uvec = [xd, ud, V, W*0]\n", "lqr_resp = ct.input_output_response(lqr_clsys, timepts, uvec, x0)\n", "U = lqr_resp.outputs[6:8] # controller input signals\n", "Y = lqr_resp.outputs[0:3] + W # noisy output signals" ] }, { "cell_type": "code", "execution_count": null, "id": "d683767f", "metadata": {}, "outputs": [], "source": [ "mhe_timepts = timepts[0:10]\n", "oep = opt_.OptimalEstimationProblem(\n", " dsys, mhe_timepts, traj_cost, terminal_cost=init_cost)\n", "mhe = oep.create_mhe_iosystem(2)\n", " \n", "mhe_resp = ct.input_output_response(\n", " mhe, timepts, [Y, U], X0=x0, \n", " params={'verbose': True}\n", ")\n", "plot_state_comparison(timepts, mhe_resp.states, lqr_resp.states)" ] }, { "cell_type": "code", "execution_count": null, "id": "bfc68072", "metadata": {}, "outputs": [], "source": [ "# Resimulate starting at the origin and moving to the \"initial\" condition\n", "uvec = [x0, ue, V, W*0]\n", "lqr_resp = ct.input_output_response(lqr_clsys, timepts, uvec, xe)\n", "U = lqr_resp.outputs[6:8] # controller input signals\n", "Y = lqr_resp.outputs[0:3] + W # noisy output signals" ] }, { "cell_type": "code", "execution_count": null, "id": "49213d04", "metadata": {}, "outputs": [], "source": [ "mhe_timepts = timepts[0:8]\n", "oep = opt_.OptimalEstimationProblem(\n", " dsys, mhe_timepts, traj_cost, terminal_cost=init_cost)\n", "mhe = oep.create_mhe_iosystem(2)\n", " \n", "mhe_resp = ct.input_output_response(\n", " mhe, timepts, [Y, U],\n", " params={'verbose': True}\n", ")\n", "plot_state_comparison(timepts, mhe_resp.outputs, lqr_resp.states)" ] }, { "cell_type": "code", "execution_count": null, "id": "650a559a", "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "language_info": { "name": "python" } }, "nbformat": 4, "nbformat_minor": 5 }