SimpleKalmanFilter.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Below code is adapted from https://github.com/denyssene/SimpleKalmanFilter"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Original C Implementation Header\n",
    "```c\n",
    "/* \n",
    " * SimpleKalmanFilter - a Kalman Filter implementation for single variable models.\n",
    " * Created by Denys Sene, January, 1, 2017.\n",
    " * Released under MIT License - see LICENSE file for details.\n",
    " */\n",
    "\n",
    "#ifndef SimpleKalmanFilter_h\n",
    "#define SimpleKalmanFilter_h\n",
    "\n",
    "class SimpleKalmanFilter \n",
    "{\n",
    "\n",
    "public:\n",
    "  SimpleKalmanFilter(float mea_e, float est_e, float q);\n",
    "  float updateEstimate(float mea);\n",
    "  void setMeasurementError(float mea_e);\n",
    "  void setEstimateError(float est_e);\n",
    "  void setProcessNoise(float q);\n",
    "  float getKalmanGain();\n",
    "  \n",
    "private:\n",
    "  float _err_measure;\n",
    "  float _err_estimate;\n",
    "  float _q;\n",
    "  float _current_estimate;\n",
    "  float _last_estimate;\n",
    "  float _kalman_gain;\n",
    "  \n",
    "};\n",
    "\n",
    "#endif\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "mutable struct SimpleKalmanFilter{T<:Real}\n",
    "    err_measure::T\n",
    "    err_estimate::T\n",
    "    q::T\n",
    "    current_estimate::T\n",
    "    last_estimate::T\n",
    "    kalman_gain::T\n",
    "end\n",
    "\n",
    "# functions to implement in Julia\n",
    "# SimpleKalmanFilter(float mea_e, float est_e, float q);\n",
    "# float updateEstimate(float mea);\n",
    "# void setMeasurementError(float mea_e);\n",
    "# void setEstimateError(float est_e);\n",
    "# void setProcessNoise(float q);\n",
    "# float getKalmanGain();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Original C Implementation Source\n",
    "```c\n",
    "/* \n",
    " * SimpleKalmanFilter - a Kalman Filter implementation for single variable models.\n",
    " * Created by Denys Sene, January, 1, 2017.\n",
    " * Released under MIT License - see LICENSE file for details.\n",
    " */\n",
    "\n",
    "#include \"Arduino.h\"\n",
    "#include \"SimpleKalmanFilter.h\"\n",
    "#include <math.h>\n",
    "\n",
    "SimpleKalmanFilter::SimpleKalmanFilter(float mea_e, float est_e, float q)\n",
    "{\n",
    "  _err_measure=mea_e;\n",
    "  _err_estimate=est_e;\n",
    "  _q = q;\n",
    "}\n",
    "\n",
    "float SimpleKalmanFilter::updateEstimate(float mea)\n",
    "{\n",
    "  _kalman_gain = _err_estimate/(_err_estimate + _err_measure);\n",
    "  _current_estimate = _last_estimate + _kalman_gain * (mea - _last_estimate);\n",
    "  _err_estimate =  (1.0 - _kalman_gain)*_err_estimate + fabs(_last_estimate-_current_estimate)*_q;\n",
    "  _last_estimate=_current_estimate;\n",
    "\n",
    "  return _current_estimate;\n",
    "}\n",
    "\n",
    "void SimpleKalmanFilter::setMeasurementError(float mea_e)\n",
    "{\n",
    "  _err_measure=mea_e;\n",
    "}\n",
    "\n",
    "void SimpleKalmanFilter::setEstimateError(float est_e)\n",
    "{\n",
    "  _err_estimate=est_e;\n",
    "}\n",
    "\n",
    "void SimpleKalmanFilter::setProcessNoise(float q)\n",
    "{\n",
    "  _q=q;\n",
    "}\n",
    "\n",
    "float SimpleKalmanFilter::getKalmanGain() {\n",
    "  return _kalman_gain;\n",
    "}\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# `SimpleKalmanFilter`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "SimpleKalmanFilter"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function SimpleKalmanFilter(mea_e::Real, est_e::Real, q::Real)\n",
    "    SimpleKalmanFilter(\n",
    "        Float32[\n",
    "            mea_e, # err_measure::T\n",
    "            est_e, # err_estimate::T\n",
    "                q, # q::T\n",
    "                0, # current_estimate::T\n",
    "                0, # last_estimate::T\n",
    "                0  # kalman_gain::T\n",
    "        ]...\n",
    "    )\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# `updateEstimate!`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "updateEstimate! (generic function with 1 method)"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function updateEstimate!(skf::SimpleKalmanFilter, mea::Real)\n",
    "    skf.kalman_gain      = skf.err_estimate / (skf.err_estimate + skf.err_measure);\n",
    "    skf.current_estimate = skf.last_estimate + skf.kalman_gain * (mea - skf.last_estimate);\n",
    "    skf.err_estimate     = (1.0 - skf.kalman_gain)*skf.err_estimate + abs(skf.last_estimate-skf.current_estimate)*skf.q;\n",
    "    skf.last_estimate    = sfk.current_estimate;\n",
    "    return skf.current_estimate;\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# `setMeasurementError!`\n",
    "# `setEstimateError!`\n",
    "# `setProcessNoise!`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "setProcessNoise! (generic function with 1 method)"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "setMeasurementError!(skf::SimpleKalmanFilter, mea_e::Real) = sfk.err_measure=mea_e\n",
    "setEstimateError!(skf::SimpleKalmanFilter, est_e::Real) = sfk.err_estimate=est_e\n",
    "setProcessNoise!(skf::SimpleKalmanFilter, q::Real) = sfk.q=q"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# `getKalmanGain`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "getKalmanGain (generic function with 1 method)"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "getKalmanGain(skf::SimpleKalmanFilter) = skf.kalman_gain"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Julia 0.6.4",
   "language": "julia",
   "name": "julia-0.6"
  },
  "language_info": {
   "file_extension": ".jl",
   "mimetype": "application/julia",
   "name": "julia",
   "version": "0.6.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}