solaruav_28_02-checkpoint.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import scipy.integrate as integrate\n",
    "import scipy.misc as diff\n",
    "import math"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "#take these as raw_input from GPS module - I've set it to 0 for now. Or do we get this as a tuple from GPS? we'll modify accordingly\n",
    "Vb=0 #airspeed bias\n",
    "Wn=0 #north component\n",
    "We=0 #east component\n",
    "Vg=0 #ground velocity\n",
    "gta=0 #ground track angle\n",
    "Vtsensor=0#airspeed reading from sensor\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#I am intiating all intial kalman filters values to zero\n",
    "V_tsensor=0#true airspeed estimate of sensor reading \n",
    "AIRSerr=0#error in air speed measurment from the sensor reading andkalman filter estimate"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [],
   "source": [
    "Xwind=np.matrix([[Vb],[Wn],[We]]) #init filter states airspeed bias, north & east 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"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#loop starts here "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "Pwind=Pwind+Qwind#integrate here: Pwind+ = Pwind+ + Qwind"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "Vn=Vg*np.cos(gta) #ground track angle\n",
    "Ve=Vg*np.sin(gta)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "Nerr=Vn-Wn #error in north component\n",
    "Eerr=Ve-We #error in east component"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [],
   "source": [
    "Va=abs(math.sqrt(Nerr*Nerr+Eerr*Eerr))#airspeed magnitude component"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "V_tsensor=Va-Vb#true airspeed estimate of sensor reading\n",
    "AIRSerr=Vtsensor-V_tsensor #AIRSerr=epsilon "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Performing standard kalman filter calculations for the |va|>0\n",
    "if(Va>0):\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",
    "    \n",
    "    #performing the covarience and state updates\n",
    "    Xwind=Xwind+K.dot(AIRSerr)\n",
    "    Pwind=Pwind-K.dot(C).dot(Pwind)\n",
    "    \n",
    "    #outputs of wind estimator\n",
    "    Wn=Xwind[1]\n",
    "    We=xwind[2]\n",
    "    Vw=abs(math.sqrt(Wn*Wn+We*We))\n",
    "    \n",
    "    \n",
    "    \n",
    "    \n",
    "            "
   ]
  },
  {
   "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
}