Trilateration Using Limited Simulated Points With scipy.optimize.least_squares.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       " active_mask: array([0., 0., 0., 0.])\n",
       "        cost: 560510320.5721085\n",
       "         fun: array([-17773.98076037,  -3051.15422548,  14861.73043898,  12966.50838412,\n",
       "         6985.62716503,   2311.65632953,   5012.11925117,   6095.35577034,\n",
       "       -17040.47805727])\n",
       "        grad: array([   2.72018811,    1.97675005,  -46.0926277 , -351.37378233])\n",
       "         jac: array([[ 2005.86507774,  1986.1609892 ,  1484.15410055,  3200.33322531],\n",
       "       [ 2005.86507774,   -13.83902035,  1364.15407756,  2428.33109372],\n",
       "       [ 2005.86507774, -2013.83901422,  1050.15417779,  3020.32044885],\n",
       "       [    5.86507994,  1986.1609892 ,   550.15413927,  2048.34011541],\n",
       "       [    5.86507799,   -13.83901447,  1416.15417917,  1406.33489744],\n",
       "       [    5.86507994, -2013.83901422,   590.15420422,  2096.33556585],\n",
       "       [-1994.13491787,  1986.1609892 ,  1066.15434125,  3006.33953611],\n",
       "       [-1994.13492567,   -13.83901251,  1246.15420388,  2346.32963052],\n",
       "       [-1994.13493347, -2013.83901422,   962.15413801,  3004.34469856]])\n",
       "     message: '`ftol` termination condition is satisfied.'\n",
       "        nfev: 6\n",
       "        njev: 6\n",
       "  optimality: 351.3737823292613\n",
       "      status: 2\n",
       "     success: True\n",
       "           x: array([ 2.00293252e+03,  1.99308048e+03, -9.09228996e+01,  1.83285601e+00])"
      ]
     },
     "execution_count": 1,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import scipy\n",
    "from scipy.optimize import least_squares\n",
    "\n",
    "# Test point is 2000, 2000, -100, 0\n",
    "\n",
    "x1, y1, z1, dist_1 = ( 1000.0, 1000.0, -833 ,1602)\n",
    "x2, y2, z2, dist_2 = ( 1000.0, 2000.0, -773 ,1216)\n",
    "x3, y3, z3, dist_3 = ( 1000.0, 3000.0, -616 ,1512)\n",
    "x4, y4, z4, dist_4 = ( 2000.0, 1000.0, -366 ,1026)\n",
    "x5, y5, z5, dist_5 = ( 2000.0, 2000.0, -799,705)\n",
    "x6, y6, z6, dist_6 = ( 2000.0, 3000.0, -386 ,1050)\n",
    "x7, y7, z7, dist_7 = ( 3000.0, 1000.0, -624,1505)\n",
    "x8, y8, z8, dist_8 = ( 3000.0, 2000.0, -714 ,1175)\n",
    "x9, y9, z9, dist_9 = ( 3000.0, 3000.0, -572 ,1504)\n",
    "\n",
    "\n",
    "s1 = 1535\n",
    "\n",
    "\n",
    "#Define a function that evaluates the equations\n",
    "def equations( guess ):\n",
    "    x, y, z, r = guess\n",
    "    \n",
    " \n",
    "    return (\n",
    "        (x - x1)**2 + (y - y1)**2 + (z - z1)**2 - (dist_1 - r )**2,\n",
    "        (x - x2)**2 + (y - y2)**2 + (z - z2)**2 - (dist_2 - r )**2,\n",
    "        (x - x3)**2 + (y - y3)**2 + (z - z3)**2 - (dist_3 - r )**2,\n",
    "        (x - x4)**2 + (y - y4)**2 + (z - z4)**2 - (dist_4 - r )**2,\n",
    "        (x - x5)**2 + (y - y5)**2 + (z - z5)**2 - (dist_5 - r )**2,\n",
    "        (x - x6)**2 + (y - y6)**2 + (z - z6)**2 - (dist_6 - r )**2,\n",
    "        (x - x7)**2 + (y - y7)**2 + (z - z7)**2 - (dist_7 - r )**2,\n",
    "        (x - x8)**2 + (y - y8)**2 + (z - z8)**2 - (dist_8 - r )**2,\n",
    "        (x - x9)**2 + (y - y9)**2 + (z - z9)**2 - (dist_9 - r )**2\n",
    "    )\n",
    "        \n",
    "\n",
    "\n",
    "# Make SciPy solve the system using an initial guess.\n",
    "# The initial guess affects which of the \"candidates\" SciPy finds.\n",
    "initial_guess = (1000, 1000, -100, 0)\n",
    "\n",
    "#equations( initial_guess )\n",
    "results = least_squares(equations, initial_guess)\n",
    "\n",
    "results\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 2.00293252e+03,  1.99308048e+03, -9.09228996e+01,  1.83285601e+00])"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "results.x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "#covar = (J^T J)^{-1}\n",
    "import numpy as np\n",
    "J = results.jac\n",
    "JT = results.jac.transpose()\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style scoped>\n",
       "    .dataframe tbody tr th:only-of-type {\n",
       "        vertical-align: middle;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: right;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>0</th>\n",
       "      <th>1</th>\n",
       "      <th>2</th>\n",
       "      <th>3</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>0</th>\n",
       "      <td>4.166613e-08</td>\n",
       "      <td>-1.368951e-03</td>\n",
       "      <td>7.662466e-07</td>\n",
       "      <td>1.396179e-06</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>1</th>\n",
       "      <td>-1.368951e-03</td>\n",
       "      <td>4.166367e-08</td>\n",
       "      <td>1.160962e-06</td>\n",
       "      <td>-2.265394e-05</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>2</th>\n",
       "      <td>7.662466e-07</td>\n",
       "      <td>1.160962e-06</td>\n",
       "      <td>8.742616e-08</td>\n",
       "      <td>4.063453e-08</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>3</th>\n",
       "      <td>1.396179e-06</td>\n",
       "      <td>-2.265394e-05</td>\n",
       "      <td>4.063453e-08</td>\n",
       "      <td>1.683536e-08</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>"
      ],
      "text/plain": [
       "              0             1             2             3\n",
       "0  4.166613e-08 -1.368951e-03  7.662466e-07  1.396179e-06\n",
       "1 -1.368951e-03  4.166367e-08  1.160962e-06 -2.265394e-05\n",
       "2  7.662466e-07  1.160962e-06  8.742616e-08  4.063453e-08\n",
       "3  1.396179e-06 -2.265394e-05  4.063453e-08  1.683536e-08"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import pandas as pd\n",
    "\n",
    "\n",
    "cov_matrix = pd.DataFrame((JT.dot(J))**-1)\n",
    "cov_matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/html": [
       "<div>\n",
       "<style scoped>\n",
       "    .dataframe tbody tr th:only-of-type {\n",
       "        vertical-align: middle;\n",
       "    }\n",
       "\n",
       "    .dataframe tbody tr th {\n",
       "        vertical-align: top;\n",
       "    }\n",
       "\n",
       "    .dataframe thead th {\n",
       "        text-align: right;\n",
       "    }\n",
       "</style>\n",
       "<table border=\"1\" class=\"dataframe\">\n",
       "  <thead>\n",
       "    <tr style=\"text-align: right;\">\n",
       "      <th></th>\n",
       "      <th>0</th>\n",
       "      <th>1</th>\n",
       "    </tr>\n",
       "  </thead>\n",
       "  <tbody>\n",
       "    <tr>\n",
       "      <th>0</th>\n",
       "      <td>4.166613e-08</td>\n",
       "      <td>-1.368951e-03</td>\n",
       "    </tr>\n",
       "    <tr>\n",
       "      <th>1</th>\n",
       "      <td>-1.368951e-03</td>\n",
       "      <td>4.166367e-08</td>\n",
       "    </tr>\n",
       "  </tbody>\n",
       "</table>\n",
       "</div>"
      ],
      "text/plain": [
       "              0             1\n",
       "0  4.166613e-08 -1.368951e-03\n",
       "1 -1.368951e-03  4.166367e-08"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "cov_mat_xy = cov_matrix.drop([2,3], 1)\n",
    "cov_mat_xy = cov_mat_xy.drop([2,3], 0)\n",
    "cov_mat_xy\n",
    "\n",
    "#http://www.visiondummy.com/2014/04/draw-error-ellipse-representing-covariance-matrix/"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([ 0.00136899, -0.00136891]), array([[ 0.70710678,  0.70710678],\n",
       "        [-0.70710678,  0.70710678]]))"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "from numpy import linalg as LA\n",
    "\n",
    "LA.eig(cov_mat_xy)"
   ]
  }
 ],
 "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.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}