kalfilter1.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np \n",
    "import math \n",
    "import scipy.signal as signal\n",
    "import scipy.integrate as integrate\n",
    "from scipy.misc import derivative"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "#inputs from pixhawk given on page 4 of aloft in one block, other variables we need in next one. separated by blank line\n",
    "#take these as raw_input from GPS module.Initiating it to zero.How does gps input it as a tuple?(Then accordingly we have to modify it)\n",
    "Vb=0#airspeed Bias, not in list\n",
    "\n",
    "autotime=0 #autopilot time\n",
    "waypn=0 #waypoint number\n",
    "ro_a=0 #roll angle\n",
    "pi_a=0 #pitch angle\n",
    "yw_a=0 #yaw angle\n",
    "ro_r=0 #roll rate\n",
    "pi_r=0 #pitch rate\n",
    "yw_r=0 #yaw rate\n",
    "lat=0\n",
    "lon=0\n",
    "balt=0\n",
    "Vn=0\n",
    "Ve=0\n",
    "Vd=0\n",
    "rpm=0\n",
    "Wn=0#Northern wind component \n",
    "We=0#Eastern wind component\n",
    "Vg=0#Ground Velocity\n",
    "gta=0#ground track angle \n",
    "Vtsensor=0#airspeed reading from sensor\n",
    "AIRSerr=0#error in air speed measurment from sensor and kalman filter estimate AIRSerr=epsilon in documentation\n",
    "waypn=0 #waypoint number\n",
    "az=0\n",
    "Vt=0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "Xwind=np.matrix([[Vb],[Wn][We]])#init filter states airspeed bias,northern and eastern wind components\n",
    "Pwind=np.matrix([[0.5,0,0],[0,0.5,0],[0,0,0.5]])\n",
    "Qwind=np.matrix([[0.0001,0,0],[0,0.001,0],[0,0,0.001]])\n",
    "Rwind=0.5\n",
    "n=0\n",
    "rinput=0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "1\n"
     ]
    }
   ],
   "source": [
    "\n",
    "#loop starts here(it is not clear on how to start the loop)\n",
    "while rinput!=1:\n",
    "    rinput=float(input())\n",
    "\n",
    "    Pwind=Pwind+Qwind#resolved, integrate was a fancy word for add \n",
    "\n",
    "    Vn=Vg*np.cos(gta)#ground track angle\n",
    "    Ve=Vg*np.sin(gta)\n",
    "\n",
    "    Nerr=Vn-Wn#error in north component\n",
    "    Eerr=Ve-We#error in east component\n",
    "    Va=abs(math.sqrt(Nerr*Nerr+Eerr*Eerr))#airspeed magnitude component \n",
    "    V_tsensor=Va-Vb#true airspeed estimate\n",
    "    AIRSerr=Vtsensor-V_tsensor\n",
    "\n",
    "    #performing the kalman filter \n",
    "\n",
    "    if(Va>0):\n",
    "        n=n+1\n",
    "        C=np.array([-1,-Nerr/Va,-Eerr/Va])\n",
    "        CT=np.transpose(C)\n",
    "        P1=np.linalg.inv(C.dot(Pwind).dot(CT).dot(Rwind))\n",
    "        K=Pwind.dot(CT).dot(P1)\n",
    "        print(\"Over here\")\n",
    "\n",
    "\n",
    "        #Performing the covarience and state updates\n",
    "        Xwind=Xwind+K.dot(AiRSerr)\n",
    "        Pwind=Pwind_K.dot(C).dot(Pwind)\n",
    "        print(\"Reached here\")\n",
    "\n",
    "        #outputs of the wind estimator\n",
    "        Wn=Xwind[1]\n",
    "        We=Xwind[2]\n",
    "        Vw=abs(math.sqrt(Wn*Wn+We*We))\n",
    "        Xw=math.atan2(-We,-Wn)#Xw represents the wind direction\n",
    "        Whw=-Wn*math.cos(Xw)-We*math.cos(Xw)#Whw represents instantaneous head wind\n",
    "        Vb=Xwind[0]\n",
    "        Vt=Vb+V_tsensor#this is a much more cleaner version of true airspeed being used and will last the rest of the program for the iteration\n",
    "    if(n==1):\n",
    "        rinput=1\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "#drag\n",
    "mac=1\n",
    "mpolar=1\n",
    "Vsink=-0.0176*Vt*Vt+0.3782*Vt-2.4993\n",
    "k=np.sqrt(mac/mpolar)\n",
    "Vsinkh=k*Vsink\n",
    "Vtheavy=k*Vt\n",
    "n=-abs(az)/9.81\n",
    "#drag coeffts very shady, do this later"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 0.1133557 , -0.13816974,  0.23206864,  0.05799814,  0.62293601,\n",
       "        0.88466184,  0.6654094 ])"
      ]
     },
     "execution_count": 28,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "a=[2,1]\n",
    "b=[1]\n",
    "x=[1,0,1,2,5,7,6]\n",
    "signal.filtfilt(b,a,x)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}