{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Source localization problem\n",
    "\n",
    "Consider $m$ sensors at known locations $a_{i}\\in\\mathbb{R}^{n}$ (in practical applications, $n=2$ or $3$). A source located at unknown location $x\\in\\mathbb{R}^{n}$ is emitting signal. From the strength of the signal received by the sensors, it is possible to get noisy measurement of the range $r_{i}$ between the source and the $i$-th sensor:\n",
    "\n",
    "$$r_{i} \\:=\\: \\parallel x - a_{i} \\parallel_{2} \\:+\\: \\varepsilon_{i}, \\quad i=1,...,m,$$\n",
    "\n",
    "where $(\\varepsilon_{1}, ..., \\varepsilon_{m})^{\\top}$ denotes the unknown noise vector. We would like to estimate the source location $x\\in\\mathbb{R}^{n}$ from the noisy measurements $r_{i}>0$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "(a) (10+10+10=30 points) A natural way to formulate the source localization problem is to cast it as:\n",
    "\n",
    "$$\\underset{x\\in\\mathbb{R}^{n}}{\\text{minimize}}\\:\\displaystyle\\sum_{i=1}^{m}\\left(r_{i} - \\parallel x - a_{i} \\parallel_{2}\\:\\right)^{2},$$\n",
    "\n",
    "or equivalently,\n",
    "\n",
    "$$\\begin{aligned}\n",
    "&\\underset{x\\in\\mathbb{R}^{n},g\\in\\mathbb{R}^{m}}{\\text{minimize}}\\:\\displaystyle\\sum_{i=1}^{m}\\left(r_{i} - g_{i}\\right)^{2},\\\\\n",
    "&\\text{subject to}\\quad g_{i}^{2} \\:=\\: \\parallel x - a_{i} \\parallel_{2}^{2}, \\quad i=1,...,m,\n",
    "\\end{aligned}$$\n",
    "\n",
    "which is a nonconvex problem. \n",
    "\n",
    "(a.1) Use the change of variables \n",
    "\n",
    "$$G := \\begin{pmatrix}g\\\\1\n",
    "\\end{pmatrix}\\left(g^{\\top} \\; 1\\right), \\quad X := \\begin{pmatrix}x\\\\1\n",
    "\\end{pmatrix}\\left(x^{\\top} \\; 1\\right),\n",
    "$$\n",
    "\n",
    "to transcribe the above problem as an optimization problem over the matrix variable pair $(X,G)$. \n",
    "\n",
    "(a.2) Is the resulting optimization problem in (a.1) convex? Why/why not? \n",
    "\n",
    "(a.3) If you think the problem in (a.1) is a convex optimization problem, then what kind is it? If you think it is not, then derive a (sub-optimal) convex relaxation of this problem."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Solution:\n",
    "(a.1) The suggested change of variables, being outer products, introduce constraints $G \\succeq 0$, $X \\succeq 0$, $\\text{rank}(G) = \\text{rank}(X) = 1$, $G_{m+1,m+1} = X_{n+1,n+1}=1$. Notice that these constraints are simply a consequence of how the new variables are defined, and have nothing to do with the optimization problem in hand. Therefore, the nonconvex problem in vector pair $(x,g)\\in \\mathbb{R}^{n}\\times\\mathbb{R}^{m}$ becomes the following problem in matrix pair $(X,G) \\in \\mathbb{S}^{n+1}_{+} \\times \\mathbb{S}^{m+1}_{+}$:\n",
    "\n",
    "$$\\begin{aligned}\n",
    "&\\underset{X\\in\\mathbb{S}^{n+1}_{+},G\\in\\mathbb{S}^{m+1}_{+}}{\\text{minimize}}\\:\\displaystyle\\sum_{i=1}^{m}\\left(G_{ii} - 2 r_{i}G_{m+1,i} + r_{i}^{2}\\right)\\\\\n",
    "&\\text{subject to}\\quad G_{ii} = \\text{trace}\\left(F_{i}X\\right), \\quad i=1,...,m,\\\\\n",
    "&\\qquad\\qquad\\;\\; G_{m+1,m+1} = X_{n+1,n+1} = 1,\\\\\n",
    "&\\qquad\\qquad\\;\\; \\text{rank}(X) = \\text{rank}(G) = 1,\n",
    "\\end{aligned}$$\n",
    "where\n",
    "$$F_{i} := \\begin{pmatrix} I_{n\\times n} & -a_{i}\\\\\n",
    "-a_{i}^{\\top} & \\parallel a_{i} \\parallel_{2}^{2}\\end{pmatrix}, \\qquad i=1,...,m.$$\n",
    "\n",
    "\n",
    "(a.2) The optimization problem derived in (a.1) in $(X,G)$ variables is still nonconvex. This is because the rank constraints are nonconvex.\n",
    "\n",
    "\n",
    "(a.3) Dropping the nonconvex rank constraints, result in the following SDP (hence convex) relaxation:\n",
    "\n",
    "$$\\begin{aligned}\n",
    "&\\underset{X\\in\\mathbb{S}^{n+1}_{+},G\\in\\mathbb{S}^{m+1}_{+}}{\\text{minimize}}\\:\\displaystyle\\sum_{i=1}^{m}\\left(G_{ii} - 2 r_{i}G_{m+1,i} + r_{i}^{2}\\right)\\\\\n",
    "&\\text{subject to}\\quad G_{ii} = \\text{trace}\\left(F_{i}X\\right), \\quad i=1,...,m,\\\\\n",
    "&\\qquad\\qquad\\;\\; G_{m+1,m+1} = X_{n+1,n+1} = 1.\n",
    "\\end{aligned}$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "(b) (15 points) In general, solving the formulation given in part (a) is difficult. A different approach is to consider the \"squared range\", instead of \"range\", as the measurement. This amounts to solving\n",
    "\n",
    "$$\\underset{x\\in\\mathbb{R}^{n}}{\\text{minimize}}\\:\\underbrace{\\displaystyle\\sum_{i=1}^{m}\\left(\\parallel x - a_{i} \\parallel_{2}^{2} \\: - \\: r_{i}^{2} \\right)^{2}}_{f_{0}(x)},$$\n",
    "\n",
    "or equivalently,\n",
    "\n",
    "$$\\begin{aligned}\n",
    "&\\underset{x\\in\\mathbb{R}^{n}, \\alpha\\in\\mathbb{R}}{\\text{minimize}}\\:\\displaystyle\\sum_{i=1}^{m}\\left(\\alpha - 2a_{i}^{\\top}x + \\parallel a_{i} \\parallel_{2}^{2} - r_{i}^{2}\\right)^{2}\\\\\n",
    "&\\text{subject to}\\quad x^{\\top}x \\:=\\: \\alpha.\n",
    "\\end{aligned}$$\n",
    "\n",
    "Introducing $y := \\begin{pmatrix}x\\\\ \\alpha\\end{pmatrix}\\in\\mathbb{R}^{n+1}$, rewrite the above problem as\n",
    "\n",
    "$$\\begin{aligned}\n",
    "&\\underset{y\\in\\mathbb{R}^{n+1}}{\\text{minimize}}\\:\\parallel Ay - b\\parallel_{2}^{2}\\\\\n",
    "&\\text{subject to}\\quad y^{\\top}Cy + 2d^{\\top}y \\:=\\: 0.\n",
    "\\end{aligned}$$\n",
    "\n",
    "In other words, express $A, b, C, d$ as function of the problem data: $r_{1}^{2},...,r_{m}^{2}$, and $a_{1},...,a_{m}$."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Solution:\n",
    "Using the suggested substitution yields\n",
    "\n",
    "$$A = \\begin{pmatrix} -2a_{1}^{\\top} & 1\\\\\n",
    "\\vdots & \\vdots\\\\\n",
    "-2a_{m}^{\\top} & 1\\end{pmatrix}, \\qquad b = \\begin{pmatrix} r_{1}^{2} - \\parallel a_{1} \\parallel_{2}^{2}\\\\ \n",
    "\\vdots\\\\\n",
    "r_{m}^{2} - \\parallel a_{m} \\parallel_{2}^{2}\n",
    "\\end{pmatrix}, \\qquad C = \\begin{pmatrix}I_{n\\times n} & 0_{n\\times 1}\\\\\n",
    "0_{1\\times 0} & 0\n",
    "\\end{pmatrix}, \\qquad d = \\begin{pmatrix}0_{n\\times 1}\\\\-0.5\\end{pmatrix}.$$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "(c) (10 points) Is the optimization problem derived in part (b) convex? Why/why not?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Solution:\n",
    "The problem derived in part (b) is a nonconvex QCQP (a generalized trust region problem). The nonconvexity is due to the fact that the quadratic constraint is an equality, hence defines a nonconvex set."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "(d) (20 points) Consider the problem derived in part (b) as primal problem. This primal problem enjoys zero duality gap since it amounts to minimizing a quadratic function subject to single quadratic constraint (generalized trust-region problem, see e.g., Lecture 12 notes, p.11-12 for trust region problem). \n",
    "\n",
    "Thanks to zero duality gap, one possible line of attack to solve the primal in part (b), is to first derive its Lagrange dual problem, solve that via cvxpy, and then invoke strong duality. Using this strategy, compute the optimal estimate of the source location $x^{\\rm{opt}}\\in\\mathbb{R}^{2}$, for $m=5$ sensors located at\n",
    "\n",
    "$$a_{1} = \\begin{pmatrix} 1.8\\\\2.5\\end{pmatrix}, \\quad a_{2} = \\begin{pmatrix} 2.0\\\\1.7\\end{pmatrix}, \\quad a_{3} = \\begin{pmatrix} 1.5\\\\1.5\\end{pmatrix}, \\quad a_{4} = \\begin{pmatrix} 1.5\\\\2.0\\end{pmatrix}, \\quad a_{5} = \\begin{pmatrix} 2.5\\\\1.5\\end{pmatrix},$$\n",
    "\n",
    "with $r = (2.00, 1.24, 0.59, 1.31, 1.44)$. Please supply your code in the notebook. \n",
    "\n",
    "For the above data, the following figure (credit: Boyd-Vanderberghe) shows the contour lines for the objective $f_{0}(x)$ in part (b) with sensor locations $a_{i}$ indicated by circles. \n",
    "\n",
    "<img width=\"350\" src=\"AMS229F18HW6.png\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Solution:\n",
    "Let $\\nu\\in\\mathbb{R}$ be the Lagrange multiplier for the equality constraint. Then the Lgrangian $L(y,\\nu) = \\parallel Ay - b\\parallel_{2}^{2} + \\nu\\left(y^{\\top}Cy + 2d^{\\top}y\\right)$ yields the dual function\n",
    "\n",
    "$$\\begin{align}\n",
    "g(\\nu) = \\begin{cases}\n",
    "-\\left(A^{\\top}b - \\nu d\\right)^{\\top}\\left(A^{\\top}A + \\nu C\\right)^{\\dagger}\\left(A^{\\top}b - \\nu d\\right) + b^{\\top}b, & \\text{if}\\quad A^{\\top}A + \\nu C \\succeq 0,\\\\\n",
    "-\\infty, & \\text{otherwise.}\n",
    "\\end{cases}\n",
    "\\end{align}$$\n",
    "\n",
    "Therefore, the dual problem is\n",
    "\n",
    "$$\\begin{aligned}\n",
    "&\\underset{\\nu\\in\\mathbb{R}}{\\text{minimize}}\\:\\left(A^{\\top}b - \\nu d\\right)^{\\top}\\left(A^{\\top}A + \\nu C\\right)^{\\dagger}\\left(A^{\\top}b - \\nu d\\right) - b^{\\top}b\\\\\n",
    "&\\text{subject to}\\quad A^{\\top}A + \\nu C \\succeq 0.\n",
    "\\end{aligned}$$\n",
    "\n",
    "The cvxpy code to solve the above is given below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "('Optimal source location is xopt = ', array([ 1.32697669,  0.64472181]))\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import numpy.linalg as la\n",
    "import cvxpy as cvx\n",
    "import cvxopt\n",
    "\n",
    "n = 2 # dimension\n",
    "m = 5 # number of sensors\n",
    "# problem data\n",
    "a = np.array([[1.8, 2.5], \n",
    "              [2.0, 1.7], \n",
    "              [1.5, 1.5], \n",
    "              [1.5,2.0], \n",
    "              [2.5,1.5]])\n",
    "r = np.array([2.00, 1.24, 0.59, 1.31, 1.44])\n",
    "# parameters in part (b) formulation as function of the problem data\n",
    "A = np.hstack([-2*a, np.ones((m,1))])\n",
    "b = r**2 - (la.norm(a,axis=1)**2)\n",
    "C = np.eye(n+1); C[-1,-1] = 0\n",
    "d = np.zeros(n+1); d[-1] = -0.5\n",
    "\n",
    "# the dual problem\n",
    "nu = cvx.Variable()\n",
    "\n",
    "objective = cvx.Maximize( - cvx.atoms.matrix_frac(np.matmul(A.T,b) - nu*d, \n",
    "                                       np.matmul(A.T,A) + nu*C) + np.matmul(b.T,b))\n",
    "\n",
    "constraints = [np.matmul(A.T, A) + nu*C >> 0]\n",
    "\n",
    "# solve using CVXOPT\n",
    "prob = cvx.Problem(objective, constraints)\n",
    "result = prob.solve(solver=cvx.CVXOPT)\n",
    "\n",
    "yopt = np.matmul(la.inv(np.matmul(A.T, A) + nu.value*C), np.matmul(A.T, b) - nu.value*d)\n",
    "\n",
    "print('Optimal source location is xopt = ', yopt[:-1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "(e) (25 points) Another way to solve for the primal in part (b) is the following. Use the KKT condition to write the primal argmin $x^{\\rm{opt}}$ as an explicit function of the optimal Lagrange multiplier $\\nu^{\\rm{opt}}$. Then derive a nonlinear algebraic equation of the form $\\phi(\\nu^{\\rm{opt}}) = 0$ and solve the same using bisection method for the numerical data given in part (d). Finally, compute and compare $x^{\\rm{opt}}$ obtained from the KKT analysis with that obtained from part (d). "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Solution:\n",
    "Due to strong duality, the optimal primal-dual variable pair $(y^{\\text{opt}},\\nu^{\\text{opt}})$ for part (b) satisfies the KKT condition:\n",
    "\n",
    "$$\\begin{align}\n",
    "&\\text{(gradient condition)}\\;\\left(A^{\\top}A + \\nu^{\\text{opt}}C\\right)y^{\\text{opt}} = A^{\\top}b - \\nu^{\\text{opt}}d, \\\\ \n",
    "&\\text{(primal feasibility)}\\;\\left(y^{\\text{opt}}\\right)^{\\top} C y^{\\text{opt}} + 2 d^{\\top}y^{\\text{opt}} = 0, \\\\ \n",
    "&\\text{(dual feasibility)}\\;A^{\\top}A + \\nu^{\\text{opt}}C \\succeq 0.\n",
    "\\end{align}$$\n",
    "\n",
    "From the gradient condition, we can write $y^{\\text{opt}}$ (and hence $x^{\\text{opt}}$) in terms of $\\nu^{\\text{opt}}$ as\n",
    "\n",
    "$$y^{\\text{opt}}\\left(\\nu^{\\text{opt}}\\right) = \\left(A^{\\top}A + \\nu^{\\text{opt}}C\\right)^{\\dagger}\\left(A^{\\top}b - \\nu^{\\text{opt}}d\\right).$$\n",
    "\n",
    "Substituting the above functional relation in the primal feasibility gives a scalar nonlinear equation $\\phi(\\nu^{\\text{opt}})=0$, where $\\nu^{\\text{opt}} \\in \\mathcal{I}$; the interval $\\mathcal{I}$ consists of all $\\nu^{\\text{opt}}$ such that $A^{\\top}A + \\nu^{\\text{opt}}C \\succeq 0$ (from dual feasibility). Specifically,\n",
    "\n",
    "$$\\phi(\\nu^{\\text{opt}}) := \\left(A^{\\top}b - \\nu^{\\text{opt}}d\\right)^{\\top}\\left(A^{\\top}A + \\nu^{\\text{opt}}C\\right)^{\\dagger} C \\left(A^{\\top}A + \\nu^{\\text{opt}}C\\right)^{\\dagger}\\left(A^{\\top}b - \\nu^{\\text{opt}}d\\right) + 2 d^{\\top}\\left(A^{\\top}A + \\nu^{\\text{opt}}C\\right)^{\\dagger}\\left(A^{\\top}b - \\nu^{\\text{opt}}d\\right) = 0.$$\n",
    "\n",
    "Notice that if $A^{\\top}A + \\nu^{\\text{opt}}C \\succ 0$ (which indeed is the case for our numerical data),  then we can replace the pseudo-inverse by inverse, and in that case, the interval $\\mathcal{I} = (-1/\\lambda_{\\max}\\left((A^{\\top}A)^{-1/2}C(A^{\\top}A)^{-1/2}\\right),+\\infty)$. By direct derivative computation it can be verified that the the function $\\phi(\\cdot)$ is decreasing over the interval $\\mathcal{I}$, and thus we perform simple bisection to compute $\\nu^{\\text{opt}}$ (and thereby $x^{\\text{opt}}$) as follows."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "('From KKT condition, nuopt = ', 0.589609183371067)\n",
      "('From dual solution via CVXOPT, nuopt = ', 0.58995428439989606)\n",
      "('From KKT condition, xopt = ', array([ 1.32688595,  0.64457943]))\n",
      "('From dual solution via CVXOPT, xopt = ', array([ 1.32697669,  0.64472181]))\n"
     ]
    }
   ],
   "source": [
    "def phi(nuopt):\n",
    "    \n",
    "    yopt = np.matmul(la.inv(np.matmul(A.T, A) + nuopt*C),np.matmul(A.T,b) - nuopt*d)\n",
    "    \n",
    "    return np.matmul(np.matmul(yopt.T,C),yopt) + 2*np.matmul(d.T,yopt)\n",
    "\n",
    "def bisection(func, tol, left, right):\n",
    "    current_val  = float('inf')\n",
    "    current = 0\n",
    "    while abs(current_val) >= tol:\n",
    "        mid = (left + right)/2\n",
    "        left_val = func(left)\n",
    "        mid_val = func(mid)\n",
    "        right_val = func(right)\n",
    "        current_val = mid_val\n",
    "        current = mid\n",
    "        if left_val > 0 and right_val < 0:\n",
    "            if mid_val > 0:\n",
    "                left = mid\n",
    "            else:\n",
    "                right = mid\n",
    "        elif left_val < 0 and right_val > 0:\n",
    "            if mid_val > 0:\n",
    "                right = mid\n",
    "            else:\n",
    "                left = mid\n",
    "        else:\n",
    "            return float('-inf')\n",
    "    return current\n",
    "\n",
    "nuoptKKT = bisection(phi, 1e-8, 0.0, 10.0)\n",
    "print(\"From KKT condition, nuopt = \", nuoptKKT)\n",
    "print('From dual solution via CVXOPT, nuopt = ', nu.value)\n",
    "\n",
    "yoptKKT = np.matmul(la.inv(np.matmul(A.T, A) + nuoptKKT*C),np.matmul(A.T,b) - nuoptKKT*d)\n",
    "print('From KKT condition, xopt = ', yoptKKT[:-1])\n",
    "print('From dual solution via CVXOPT, xopt = ', yopt[:-1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.14"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}