{ "nbformat": 4, "nbformat_minor": 5, "metadata": { "kernelspec": { "display_name": "Python 3", "language": "python", "name": "python3" }, "language_info": { "name": "python", "version": "3.12.0" }, "orphan": true }, "cells": [ { "cell_type": "markdown", "id": "ekf-title", "metadata": {}, "source": [ "# Extended Kalman Filter (EKF)\n", "\n", "\u2190 [f_h Functions](../observers.ipynb)\n", "\n", "Derivation, argument reference, and worked example for the `ekf` block." ] }, { "cell_type": "markdown", "id": "ekf-overview", "metadata": {}, "source": [ "## Overview\n", "\n", "The EKF extends the Kalman filter to **nonlinear** systems by linearising the dynamics and\n", "observation functions around the current estimate at each step using user-supplied Jacobians.\n", "Everywhere the KF uses the constant matrices $F_k$ and $H_k$, the EKF substitutes their\n", "local first-order approximations $F^J_k$ and $H^J_k$.\n", "\n", "It is the workhorse estimator for mildly nonlinear systems where Jacobians are analytically\n", "available or cheap to compute." ] }, { "cell_type": "markdown", "id": "ekf-model", "metadata": {}, "source": [ "## State-Space Model\n", "\n", "The EKF assumes the system evolves as:\n", "\n", "$$x_{k+1} = f(x_k, u_k) + w_k, \\qquad w_k \\sim \\mathcal{N}(0, Q_k)$$\n", "$$y_k = h(x_k) + v_k, \\qquad v_k \\sim \\mathcal{N}(0, R_k)$$\n", "\n", "| Symbol | Space | Role |\n", "|--------|-------|------|\n", "| $x_k$ | $\\mathbb{R}^n$ | True (hidden) state |\n", "| $u_k$ | $\\mathbb{R}$ | Known control input |\n", "| $y_k$ | $\\mathbb{R}^m$ | Observed measurement |\n", "| $f(\\cdot)$ | $\\mathbb{R}^n \\to \\mathbb{R}^n$ | Nonlinear state transition |\n", "| $h(\\cdot)$ | $\\mathbb{R}^n \\to \\mathbb{R}^m$ | Nonlinear observation function |\n", "| $w_k$ | $\\mathbb{R}^n$ | Process noise, covariance $Q_k$ |\n", "| $v_k$ | $\\mathbb{R}^m$ | Measurement noise, covariance $R_k$ |" ] }, { "cell_type": "markdown", "id": "ekf-predict", "metadata": {}, "source": [ "## Predict Step\n", "\n", "Propagate the estimate through the true nonlinear function, then linearise for the covariance:\n", "\n", "$$\\hat{x}_{k|k-1} = f(\\hat{x}_{k-1}, u_k)$$\n", "$$P_{k|k-1} = F^J_k\\, P_{k-1}\\, {F^J_k}^\\top + Q_k$$\n", "\n", "where $F^J_k = \\dfrac{\\partial f}{\\partial x}\\Big|_{\\hat{x}_{k-1}}$ is the Jacobian of $f$ with respect to $x$,\n", "evaluated at the previous estimate. **This must be supplied by the caller as `Fjk`.**" ] }, { "cell_type": "markdown", "id": "ekf-update", "metadata": {}, "source": [ "## Update Step\n", "\n", "Identical in structure to the KF update, with $F^J_k$, $H^J_k$ in place of $F_k$, $H_k$,\n", "and the nonlinear $h$ used for the innovation:\n", "\n", "$$S_k = H^J_k\\, P_{k|k-1}\\, {H^J_k}^\\top + R_k$$\n", "$$K_k = P_{k|k-1}\\, {H^J_k}^\\top S_k^{-1}$$\n", "$$\\hat{x}_k = \\hat{x}_{k|k-1} + K_k\\,(y_k - h(\\hat{x}_{k|k-1}))$$\n", "$$P_k = (I - K_k H^J_k)\\,P_{k|k-1}$$\n", "\n", "where $H^J_k = \\dfrac{\\partial h}{\\partial x}\\Big|_{\\hat{x}_{k|k-1}}$. **Supplied by the caller as `Hjk`.**\n", "\n", "Note the innovation uses $h(\\hat{x}_{k|k-1})$ rather than $H^J_k \\hat{x}_{k|k-1}$ \u2014 the nonlinear\n", "function is used directly for the residual, only the covariance propagation is linearised." ] }, { "cell_type": "markdown", "id": "ekf-args", "metadata": {}, "source": [ "## Argument Reference\n", "\n", "### `zk` \u2014 EKF state   `tuple[Float[np.ndarray, \"n\"], Float[np.ndarray, \"n n\"]]`\n", "\n", "| Field | Symbol | Shape | Description |\n", "|-------|--------|-------|-------------|\n", "| `zk[0]` | $\\hat{x}_k$ | `(n,)` | Current state estimate |\n", "| `zk[1]` | $P_k$ | `(n, n)` | Error covariance \u2014 symmetric, positive semi-definite |\n", "\n", "**Initialisation:** `zk = (x0, P0)`. Same convention as the KF.\n", "\n", "---\n", "\n", "### `uk` \u2014 control input   `float`\n", "\n", "Scalar control input at step $k$. Passed directly to `f_nl(x, uk)`. Pass `0.0` if uncontrolled.\n", "\n", "---\n", "\n", "### `yk` \u2014 measurement   `Float[np.ndarray, \"m\"]`\n", "\n", "Raw sensor reading at step $k$, shape `(m,)`. Must match the output space of `h_nl`.\n", "\n", "---\n", "\n", "### `f_nl` \u2014 nonlinear dynamics   `Callable`\n", "\n", "Signature: `f_nl(x: ndarray, u: float) -> ndarray`\n", "\n", "The true nonlinear state transition. Used directly to propagate the mean in the predict step.\n", "\n", "```python\n", "def f_nl(x, u):\n", " th, w = x\n", " return np.array([th + dt * w, w + dt * (-g / L * np.sin(th) + u)])\n", "```\n", "\n", "---\n", "\n", "### `h_nl` \u2014 nonlinear observation   `Callable`\n", "\n", "Signature: `h_nl(x: ndarray) -> ndarray`\n", "\n", "The true nonlinear observation function. Used in the innovation residual $y_k - h(\\hat{x}_{k|k-1})$.\n", "\n", "```python\n", "def h_nl(x):\n", " return np.array([x[0]]) # observe only theta\n", "```\n", "\n", "---\n", "\n", "### `Fjk` \u2014 state Jacobian   `Float[np.ndarray, \"n n\"]`\n", "\n", "Shape `(n, n)`. The Jacobian of `f_nl` with respect to `x`, evaluated at $\\hat{x}_{k-1}$:\n", "\n", "$$F^J_k = \\frac{\\partial f}{\\partial x}\\bigg|_{\\hat{x}_{k-1}}$$\n", "\n", "Must be recomputed each step at the current estimate. For the pendulum:\n", "\n", "```python\n", "def pend_Fj(x):\n", " th, w = x\n", " return np.array([[1.0, dt],\n", " [-dt * g / L * np.cos(th), 1.0]])\n", "\n", "Fjk = pend_Fj(zk[0]) # evaluate at current estimate\n", "```\n", "\n", "---\n", "\n", "### `Hjk` \u2014 observation Jacobian   `Float[np.ndarray, \"m n\"]`\n", "\n", "Shape `(m, n)`. The Jacobian of `h_nl` with respect to `x`, evaluated at $\\hat{x}_{k|k-1}$:\n", "\n", "$$H^J_k = \\frac{\\partial h}{\\partial x}\\bigg|_{\\hat{x}_{k|k-1}}$$\n", "\n", "If `h_nl` is linear (e.g. observe one component), `Hjk` is constant and can be precomputed:\n", "\n", "```python\n", "Hjk = np.array([[1.0, 0.0]]) # shape (1, 2), constant for linear h\n", "```\n", "\n", "---\n", "\n", "### `Qk` \u2014 process noise covariance   `Float[np.ndarray, \"n n\"]`\n", "\n", "Shape `(n, n)`, symmetric positive semi-definite. Same role as in the KF \u2014 encodes model uncertainty.\n", "\n", "---\n", "\n", "### `Rk` \u2014 measurement noise covariance   `Float[np.ndarray, \"m m\"]`\n", "\n", "Shape `(m, m)`, symmetric positive definite. Same role as in the KF \u2014 encodes sensor noise." ] }, { "cell_type": "markdown", "id": "ekf-impl-header", "metadata": {}, "source": [ "## Implementation\n", "\n", "`ekf_f` runs the full predict\u2192update cycle using the nonlinear functions and their Jacobians.\n", "`ekf_h` extracts $\\hat{x}_k$ for downstream use.\n", "\n", "The `**kwargs` in `ekf_h` absorbs any extra arguments passed by `_smart_call` that `h` does not need." ] }, { "cell_type": "code", "id": "ekf-imports", "metadata": {}, "execution_count": null, "outputs": [], "source": [ "import numpy as np\n", "from typing import Callable\n", "from jaxtyping import Float, jaxtyped\n", "from beartype import beartype\n", "from dynamicalnodes import DynamicalSystem" ] }, { "cell_type": "code", "id": "ekf-impl", "metadata": {}, "execution_count": null, "outputs": [], "source": [ "@jaxtyped(typechecker=beartype)\n", "def ekf_f(\n", " zk: tuple[Float[np.ndarray, \"n\"], Float[np.ndarray, \"n n\"]],\n", " uk: float,\n", " yk: Float[np.ndarray, \"m\"],\n", " f_nl: Callable,\n", " h_nl: Callable,\n", " Fjk: Float[np.ndarray, \"n n\"],\n", " Hjk: Float[np.ndarray, \"m n\"],\n", " Qk: Float[np.ndarray, \"n n\"],\n", " Rk: Float[np.ndarray, \"m m\"],\n", ") -> tuple[Float[np.ndarray, \"n\"], Float[np.ndarray, \"n n\"]]:\n", " \"\"\"EKF predict+update via Jacobian linearisation; returns updated state and covariance.\n", "\n", " Args:\n", " zk: EKF state \u2014 2-tuple of (x\u0302_k, P_k), state estimate and error covariance.\n", " uk: Scalar control input.\n", " yk: Measurement vector (m,).\n", " f_nl: Nonlinear state transition f(x, u) \u2192 x_next.\n", " h_nl: Nonlinear observation function h(x) \u2192 y.\n", " Fjk: Jacobian \u2202f/\u2202x evaluated at x\u0302_{k-1} (n, n).\n", " Hjk: Jacobian \u2202h/\u2202x evaluated at x\u0302_{k|k-1} (m, n).\n", " Qk: Process noise covariance (n, n).\n", " Rk: Measurement noise covariance (m, m).\n", " \"\"\"\n", " x, P = zk\n", " x_pred = f_nl(x, uk)\n", " P_pred = Fjk @ P @ Fjk.T + Qk\n", " S = Hjk @ P_pred @ Hjk.T + Rk\n", " K = P_pred @ Hjk.T @ np.linalg.inv(S)\n", " yk = np.atleast_1d(yk)\n", " x_upd = x_pred + K @ (yk - h_nl(x_pred))\n", " P_upd = (np.eye(len(x)) - K @ Hjk) @ P_pred\n", " return (x_upd, P_upd)\n", "\n", "\n", "@jaxtyped(typechecker=beartype)\n", "def ekf_h(\n", " zk: tuple[Float[np.ndarray, \"n\"], Float[np.ndarray, \"n n\"]],\n", " **kwargs,\n", ") -> Float[np.ndarray, \"n\"]:\n", " \"\"\"Return the current state estimate from the EKF state tuple.\n", "\n", " Args:\n", " zk: EKF state \u2014 2-tuple of (x\u0302_k, P_k), state estimate and error covariance.\n", " \"\"\"\n", " return zk[0]\n", "\n", "\n", "ekf = DynamicalSystem(f=ekf_f, h=ekf_h)" ] }, { "cell_type": "markdown", "id": "ekf-usage-header", "metadata": {}, "source": [ "## Worked Example \u2014 Pendulum\n", "\n", "State `x = [theta, theta_dot]`, uncontrolled, observe angle only." ] }, { "cell_type": "code", "id": "ekf-usage", "metadata": {}, "execution_count": null, "outputs": [], "source": [ "g, L, dt = 9.81, 1.0, 0.05\n", "\n", "\n", "def pend_f(x, u):\n", " th, w = x\n", " return np.array([th + dt * w, w + dt * (-g / L * np.sin(th) + u)])\n", "\n", "\n", "def pend_h(x):\n", " return np.array([x[0]])\n", "\n", "\n", "def pend_Fj(x):\n", " th, w = x\n", " return np.array([[1.0, dt], [-dt * g / L * np.cos(th), 1.0]])\n", "\n", "\n", "Hjk = np.array([[1.0, 0.0]])\n", "Qk = np.diag([1e-4, 1e-3])\n", "Rk = np.array([[0.05]])\n", "\n", "zk = (np.array([0.1, 0.0]), np.eye(2))\n", "zk_next, x_est = ekf.step(\n", " zk=zk,\n", " uk=0.0,\n", " yk=np.array([0.09]),\n", " f_nl=pend_f,\n", " h_nl=pend_h,\n", " Fjk=pend_Fj(zk[0]),\n", " Hjk=Hjk,\n", " Qk=Qk,\n", " Rk=Rk,\n", ")\n", "print(\"estimate (pre-update):\", x_est)\n", "print(\"updated estimate: \", zk_next[0])\n", "print(\"updated covariance:\\n\", zk_next[1])" ] }, { "cell_type": "markdown", "id": "3ef2dd5d", "source": [ "#", "#", " ", "`", "D", "y", "n", "a", "m", "i", "c", "a", "l", "S", "y", "s", "t", "e", "m", "`", " ", "\u2192", " ", "`", "R", "O", "S", "N", "o", "d", "e", "`" ], "metadata": {} }, { "cell_type": "code", "id": "302843e4", "source": [ "f", "r", "o", "m", " ", "d", "y", "n", "a", "m", "i", "c", "a", "l", "n", "o", "d", "e", "s", " ", "i", "m", "p", "o", "r", "t", " ", "R", "O", "S", "N", "o", "d", "e", "\n", "f", "r", "o", "m", " ", "d", "y", "n", "a", "m", "i", "c", "a", "l", "n", "o", "d", "e", "s", ".", "r", "o", "s", "2", "p", "y", "_", "p", "y", "2", "r", "o", "s", " ", "i", "m", "p", "o", "r", "t", " ", "(", "\n", " ", " ", " ", " ", "r", "o", "s", "2", "p", "y", "_", "f", "l", "o", "a", "t", "6", "4", "_", "m", "u", "l", "t", "i", "a", "r", "r", "a", "y", ",", "\n", " ", " ", " ", " ", "r", "o", "s", "2", "p", "y", "_", "f", "l", "o", "a", "t", "6", "4", ",", "\n", " ", " ", " ", " ", "p", "y", "2", "r", "o", "s", "_", "f", "l", "o", "a", "t", "6", "4", "_", "m", "u", "l", "t", "i", "a", "r", "r", "a", "y", ",", "\n", ")", "\n", "f", "r", "o", "m", " ", "s", "t", "d", "_", "m", "s", "g", "s", ".", "m", "s", "g", " ", "i", "m", "p", "o", "r", "t", " ", "F", "l", "o", "a", "t", "6", "4", "M", "u", "l", "t", "i", "A", "r", "r", "a", "y", ",", " ", "F", "l", "o", "a", "t", "6", "4", "\n", "\n", "e", "k", "f", "_", "n", "o", "d", "e", " ", "=", " ", "R", "O", "S", "N", "o", "d", "e", "(", "\n", " ", " ", " ", " ", "d", "y", "n", "a", "m", "i", "c", "a", "l", "_", "s", "y", "s", "t", "e", "m", "=", "e", "k", "f", ",", "\n", " ", " ", " ", " ", "s", "u", "b", "s", "c", "r", "i", "b", "e", "s", "_", "t", "o", "=", "[", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "{", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "t", "o", "p", "i", "c", "\"", ":", " ", "\"", "/", "m", "e", "a", "s", "u", "r", "e", "m", "e", "n", "t", "\"", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "m", "s", "g", "_", "t", "y", "p", "e", "\"", ":", " ", "F", "l", "o", "a", "t", "6", "4", "M", "u", "l", "t", "i", "A", "r", "r", "a", "y", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "a", "r", "g", "\"", ":", " ", "\"", "y", "k", "\"", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "r", "o", "s", "2", "p", "y", "\"", ":", " ", "r", "o", "s", "2", "p", "y", "_", "f", "l", "o", "a", "t", "6", "4", "_", "m", "u", "l", "t", "i", "a", "r", "r", "a", "y", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "}", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "{", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "t", "o", "p", "i", "c", "\"", ":", " ", "\"", "/", "c", "o", "n", "t", "r", "o", "l", "\"", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "m", "s", "g", "_", "t", "y", "p", "e", "\"", ":", " ", "F", "l", "o", "a", "t", "6", "4", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "a", "r", "g", "\"", ":", " ", "\"", "u", "k", "\"", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "r", "o", "s", "2", "p", "y", "\"", ":", " ", "r", "o", "s", "2", "p", "y", "_", "f", "l", "o", "a", "t", "6", "4", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "}", ",", "\n", " ", " ", " ", " ", "]", ",", "\n", " ", " ", " ", " ", "p", "u", "b", "l", "i", "s", "h", "e", "s", "_", "t", "o", "=", "[", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "{", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "t", "o", "p", "i", "c", "\"", ":", " ", "\"", "/", "s", "t", "a", "t", "e", "_", "e", "s", "t", "i", "m", "a", "t", "e", "\"", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "m", "s", "g", "_", "t", "y", "p", "e", "\"", ":", " ", "F", "l", "o", "a", "t", "6", "4", "M", "u", "l", "t", "i", "A", "r", "r", "a", "y", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "p", "y", "2", "r", "o", "s", "\"", ":", " ", "p", "y", "2", "r", "o", "s", "_", "f", "l", "o", "a", "t", "6", "4", "_", "m", "u", "l", "t", "i", "a", "r", "r", "a", "y", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "}", ",", "\n", " ", " ", " ", " ", "]", ",", "\n", ")" ], "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "markdown", "id": "10933912", "source": [ "#", "#", " ", "`", "R", "O", "S", "N", "o", "d", "e", ".", "w", "r", "i", "t", "e", "_", "R", "O", "S", "N", "o", "d", "e", "_", "t", "o", "_", "r", "c", "l", "p", "y", "(", ")", "`" ], "metadata": {} }, { "cell_type": "code", "id": "287af9a1", "source": [ "g", ",", " ", "L", ",", " ", "d", "t", "_", "e", "k", "f", " ", "=", " ", "9", ".", "8", "1", ",", " ", "1", ".", "0", ",", " ", "0", ".", "1", "\n", "\n", "\n", "d", "e", "f", " ", "p", "e", "n", "d", "u", "l", "u", "m", "_", "f", "(", "x", ",", " ", "u", ")", ":", "\n", " ", " ", " ", " ", "t", "h", "e", "t", "a", ",", " ", "o", "m", "e", "g", "a", " ", "=", " ", "x", "\n", " ", " ", " ", " ", "a", "l", "p", "h", "a", " ", "=", " ", "-", "g", " ", "/", " ", "L", " ", "*", " ", "n", "p", ".", "s", "i", "n", "(", "t", "h", "e", "t", "a", ")", " ", "+", " ", "u", "\n", " ", " ", " ", " ", "r", "e", "t", "u", "r", "n", " ", "n", "p", ".", "a", "r", "r", "a", "y", "(", "[", "t", "h", "e", "t", "a", " ", "+", " ", "o", "m", "e", "g", "a", " ", "*", " ", "d", "t", "_", "e", "k", "f", ",", " ", "o", "m", "e", "g", "a", " ", "+", " ", "a", "l", "p", "h", "a", " ", "*", " ", "d", "t", "_", "e", "k", "f", "]", ")", "\n", "\n", "\n", "d", "e", "f", " ", "p", "e", "n", "d", "u", "l", "u", "m", "_", "h", "(", "x", ")", ":", "\n", " ", " ", " ", " ", "r", "e", "t", "u", "r", "n", " ", "n", "p", ".", "a", "r", "r", "a", "y", "(", "[", "x", "[", "0", "]", "]", ")", "\n", "\n", "\n", "F", "j", "k", " ", "=", " ", "n", "p", ".", "a", "r", "r", "a", "y", "(", "[", "[", "1", ".", "0", ",", " ", "d", "t", "_", "e", "k", "f", "]", ",", " ", "[", "-", "g", " ", "/", " ", "L", " ", "*", " ", "d", "t", "_", "e", "k", "f", ",", " ", "1", ".", "0", "]", "]", ")", "\n", "H", "j", "k", " ", "=", " ", "n", "p", ".", "a", "r", "r", "a", "y", "(", "[", "[", "1", ".", "0", ",", " ", "0", ".", "0", "]", "]", ")", "\n", "\n", "e", "k", "f", "_", "n", "o", "d", "e", ".", "w", "r", "i", "t", "e", "_", "R", "O", "S", "N", "o", "d", "e", "_", "t", "o", "_", "r", "c", "l", "p", "y", "(", "\n", " ", " ", " ", " ", "\"", "e", "k", "f", "_", "n", "o", "d", "e", ".", "p", "y", "\"", ",", "\n", " ", " ", " ", " ", "n", "o", "d", "e", "_", "n", "a", "m", "e", "=", "\"", "e", "x", "t", "e", "n", "d", "e", "d", "_", "k", "a", "l", "m", "a", "n", "_", "f", "i", "l", "t", "e", "r", "\"", ",", "\n", " ", " ", " ", " ", "d", "e", "p", "s", "=", "[", "p", "e", "n", "d", "u", "l", "u", "m", "_", "f", ",", " ", "p", "e", "n", "d", "u", "l", "u", "m", "_", "h", "]", ",", "\n", " ", " ", " ", " ", "i", "n", "i", "t", "i", "a", "l", "_", "s", "t", "a", "t", "e", "=", "(", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "n", "p", ".", "a", "r", "r", "a", "y", "(", "[", "0", ".", "0", ",", " ", "0", ".", "0", "]", ")", ",", " ", " ", "#", " ", "x", "\u0302", "_", "0", ":", " ", "[", "\u03b8", ",", " ", "\u03c9", "]", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "n", "p", ".", "e", "y", "e", "(", "2", ")", ",", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", " ", "#", " ", "P", "_", "0", "\n", " ", " ", " ", " ", ")", ",", "\n", " ", " ", " ", " ", "s", "t", "a", "t", "i", "c", "_", "p", "a", "r", "a", "m", "s", "=", "{", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "f", "_", "n", "l", "\"", ":", " ", "p", "e", "n", "d", "u", "l", "u", "m", "_", "f", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "h", "_", "n", "l", "\"", ":", " ", "p", "e", "n", "d", "u", "l", "u", "m", "_", "h", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "F", "j", "k", "\"", ":", " ", "F", "j", "k", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "H", "j", "k", "\"", ":", " ", "H", "j", "k", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "Q", "k", "\"", ":", " ", "n", "p", ".", "d", "i", "a", "g", "(", "[", "1", "e", "-", "4", ",", " ", "1", "e", "-", "3", "]", ")", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "R", "k", "\"", ":", " ", "n", "p", ".", "a", "r", "r", "a", "y", "(", "[", "[", "0", ".", "0", "1", "]", "]", ")", ",", "\n", " ", " ", " ", " ", "}", ",", "\n", " ", " ", " ", " ", "i", "n", "i", "t", "i", "a", "l", "_", "i", "n", "p", "u", "t", "s", "=", "{", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "y", "k", "\"", ":", " ", "n", "p", ".", "a", "r", "r", "a", "y", "(", "[", "0", ".", "0", "]", ")", ",", "\n", " ", " ", " ", " ", " ", " ", " ", " ", "\"", "u", "k", "\"", ":", " ", "0", ".", "0", ",", "\n", " ", " ", " ", " ", "}", ",", "\n", ")" ], "metadata": {}, "execution_count": null, "outputs": [] }, { "cell_type": "markdown", "id": "ekf-backlink", "metadata": {}, "source": [ "\u2190 [f_h Functions](../observers.ipynb)" ] } ] }