{ "cells": [ { "cell_type": "markdown", "metadata": {}, "source": [ "# Getting started: MHE\n", "\n", "**Open an interactive online Jupyter Notebook with this content on Binder:**\n", "\n", "[![Binder](https://mybinder.org/badge_logo.svg)](https://mybinder.org/v2/gh/do-mpc/do-mpc/master?filepath=%2Fdocumentation%2Fsource%2Fmhe_example.ipynb)\n", "\n", "In this Jupyter Notebook we illustrate application of the **do-mpc** moving horizon estimation module.\n", "Please follow first the general **Getting Started** guide, as we cover the sample example and skip over some previously explained details." ] }, { "cell_type": "code", "execution_count": 1, "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "from casadi import *\n", "\n", "# Add do_mpc to path. This is not necessary if it was installed via pip.\n", "import sys\n", "import os\n", "rel_do_mpc_path = os.path.join('..','..','..')\n", "sys.path.append(rel_do_mpc_path)\n", "\n", "# Import do_mpc package:\n", "import do_mpc" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Creating the model" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "First, we need to decide on the model type. For the given example, we are working with a continuous model." ] }, { "cell_type": "code", "execution_count": 2, "metadata": {}, "outputs": [], "source": [ "model_type = 'continuous' # either 'discrete' or 'continuous'\n", "model = do_mpc.model.Model(model_type)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The model is based on the assumption that we have additive process and/or measurement noise:\n", "\n", "\\begin{align}\n", "\\dot{x}(t) &= f(x(t),u(t),z(t),p(t),p_{\\text{tv}}(t))+w(t), \\\\\n", "y(t) &= h(x(t),u(t),z(t),p(t),p_{\\text{tv}}(t))+v(t),\n", "\\end{align}\n", "\n", "we are free to chose, which states and which measurements experience additive noise." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Model variables\n", "\n", "The next step is to define the model variables. It is important to define the variable type, name and optionally shape (default is scalar variable). \n", "\n", "In contrast to the previous example, we now use vectors for all variables.\n" ] }, { "cell_type": "code", "execution_count": 3, "metadata": { "scrolled": false }, "outputs": [], "source": [ "phi = model.set_variable(var_type='_x', var_name='phi', shape=(3,1))\n", "dphi = model.set_variable(var_type='_x', var_name='dphi', shape=(3,1))\n", "\n", "# Two states for the desired (set) motor position:\n", "phi_m_set = model.set_variable(var_type='_u', var_name='phi_m_set', shape=(2,1))\n", "\n", "# Two additional states for the true motor position:\n", "phi_m = model.set_variable(var_type='_x', var_name='phi_m', shape=(2,1))" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Model measurements\n", "\n", "This step is essential for the state estimation task: We must define a measurable output.\n", "Typically, this is a subset of states (or a transformation thereof) as well as the inputs.\n", "\n", "Note that some MHE implementations consider inputs separately.\n", "\n", "As mentionned above, we need to define for each measurement if additive noise is present.\n", "In our case we assume noisy state measurements ($\\phi$) but perfect input measurements." ] }, { "cell_type": "code", "execution_count": 4, "metadata": {}, "outputs": [], "source": [ "# State measurements\n", "phi_meas = model.set_meas('phi_1_meas', phi, meas_noise=True)\n", "\n", "# Input measurements\n", "phi_m_set_meas = model.set_meas('phi_m_set_meas', phi_m_set, meas_noise=False)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Model parameters\n", "\n", "Next we **define parameters**. The MHE allows to estimate parameters as well as states. Note that not all parameters must be estimated (as shown in the MHE setup below). We can also hardcode parameters (such as the spring constants `c`).\n", "\n" ] }, { "cell_type": "code", "execution_count": 5, "metadata": {}, "outputs": [], "source": [ "Theta_1 = model.set_variable('parameter', 'Theta_1') \n", "Theta_2 = model.set_variable('parameter', 'Theta_2')\n", "Theta_3 = model.set_variable('parameter', 'Theta_3')\n", "\n", "c = np.array([2.697, 2.66, 3.05, 2.86])*1e-3\n", "d = np.array([6.78, 8.01, 8.82])*1e-5" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Right-hand-side equation\n", "Finally, we set the right-hand-side of the model by calling `model.set_rhs(var_name, expr)` with the `var_name` from the state variables defined above and an expression in terms of $x, u, z, p$. \n", "\n", "Note that we can decide whether the inidividual states experience process noise. \n", "In this example we choose that the system model is perfect.\n", "This is the default setting, so we don't need to pass this parameter explictly." ] }, { "cell_type": "code", "execution_count": 6, "metadata": {}, "outputs": [], "source": [ "model.set_rhs('phi', dphi)\n", "\n", "dphi_next = vertcat(\n", " -c[0]/Theta_1*(phi[0]-phi_m[0])-c[1]/Theta_1*(phi[0]-phi[1])-d[0]/Theta_1*dphi[0],\n", " -c[1]/Theta_2*(phi[1]-phi[0])-c[2]/Theta_2*(phi[1]-phi[2])-d[1]/Theta_2*dphi[1],\n", " -c[2]/Theta_3*(phi[2]-phi[1])-c[3]/Theta_3*(phi[2]-phi_m[1])-d[2]/Theta_3*dphi[2],\n", ")\n", "\n", "model.set_rhs('dphi', dphi_next, process_noise = False)\n", "\n", "tau = 1e-2 \n", "model.set_rhs('phi_m', 1/tau*(phi_m_set - phi_m))\n" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The model setup is completed by calling `model.setup()`:" ] }, { "cell_type": "code", "execution_count": 7, "metadata": {}, "outputs": [], "source": [ "model.setup()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "After calling `model.setup()` we cannot define further variables etc." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## Configuring the moving horizon estimator\n", "The first step of configuring the moving horizon estimator is to call the class with a list of all parameters to be estimated. An empty list (default value) means that no parameters are estimated.\n", "The list of estimated parameters must be a subset (or all) of the previously defined parameters.\n", "\n", "