diff --git a/include/usgscsm/UsgsAstroSarSensorModel.h b/include/usgscsm/UsgsAstroSarSensorModel.h
index 3855911d171b88928cd8559a1eb74ee2097f141a..118f9f4eb07deaf7900027417a4f5deca7e00bb9 100644
--- a/include/usgscsm/UsgsAstroSarSensorModel.h
+++ b/include/usgscsm/UsgsAstroSarSensorModel.h
@@ -201,6 +201,8 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab
 
     double slantRangeToGroundRange(const csm::EcefCoord& groundPt, double time, double slantRange, double tolerance) const;
 
+    double groundRangeToSlantRange(double groundRange, const std::vector<double> &coeffs) const;
+
     csm::EcefVector getSpacecraftPosition(double time) const;
     csm::EcefVector getSpacecraftVelocity(double time) const;
     std::vector<double> getRangeCoefficients(double time) const;
diff --git a/include/usgscsm/Utilities.h b/include/usgscsm/Utilities.h
index 51fb78d07abab4eb8cb8812a45584199a620f0ad..b1d2abe9a4ccd0b3c253aca2d94835d2a2c0b521 100644
--- a/include/usgscsm/Utilities.h
+++ b/include/usgscsm/Utilities.h
@@ -10,6 +10,7 @@
 
 #include <json/json.hpp>
 
+#include <csm.h>
 #include <Warning.h>
 // methods pulled out of los2ecf and computeViewingPixel
 
@@ -84,6 +85,21 @@ double secantRoot(
     double epsilon = 1e-10,
     int maxIterations = 30);
 
+// Vector operations
+csm::EcefVector operator*(double scalar, const csm::EcefVector &vec);
+csm::EcefVector operator*(const csm::EcefVector &vec, double scalar);
+csm::EcefVector operator/(const csm::EcefVector &vec, double scalar);
+csm::EcefVector operator+(const csm::EcefVector &vec1, const csm::EcefVector &vec2);
+csm::EcefVector operator-(const csm::EcefVector &vec1, const csm::EcefVector &vec2);
+double dot(const csm::EcefVector &vec1, const csm::EcefVector &vec2);
+csm::EcefVector cross(const csm::EcefVector &vec1, const csm::EcefVector &vec2);
+double norm(const csm::EcefVector &vec);
+csm::EcefVector normalized(const csm::EcefVector &vec);
+// The projection of vec1 onto vec2. The component of vec1 that is parallel to vec2
+csm::EcefVector projection(const csm::EcefVector &vec1, const csm::EcefVector &vec2);
+// The rejection of vec1 onto vec2. The component of vec1 that is orthogonal to vec2
+csm::EcefVector rejection(const csm::EcefVector &vec1, const csm::EcefVector &vec2);
+
 // Methods for checking/accessing the ISD
 
 double metric_conversion(double val, std::string from, std::string to="m");
diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp
index 8f44b94c7b506436a42e20e4977f975d69633c24..c6c2bf6cf764984ea9dd39122d2442ef1a13b3ac 100644
--- a/src/UsgsAstroSarSensorModel.cpp
+++ b/src/UsgsAstroSarSensorModel.cpp
@@ -360,20 +360,15 @@ double UsgsAstroSarSensorModel::dopplerShift(
     csm::EcefCoord groundPt,
     double tolerance) const
 {
-   std::function<double(double)> dopplerShiftFunction = [this, groundPt](double time) {
+   csm::EcefVector groundVec(groundPt.x ,groundPt.y, groundPt.z);
+   std::function<double(double)> dopplerShiftFunction = [this, groundVec](double time) {
      csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
      csm::EcefVector spacecraftVelocity = getSpacecraftVelocity(time);
-     double lookVector[3];
+     csm::EcefVector lookVector = spacecraftPosition - groundVec;
 
-     lookVector[0] = spacecraftPosition.x - groundPt.x;
-     lookVector[1] = spacecraftPosition.y - groundPt.y;
-     lookVector[2] = spacecraftPosition.z - groundPt.z;
+     double slantRange = norm(lookVector);
 
-     double slantRange = sqrt(pow(lookVector[0], 2) +  pow(lookVector[1], 2) + pow(lookVector[2], 2));
-
-
-     double dopplerShift = -2.0 * (lookVector[0]*spacecraftVelocity.x + lookVector[1]*spacecraftVelocity.y
-                            + lookVector[2]*spacecraftVelocity.z)/(slantRange * m_wavelength);
+     double dopplerShift = -2.0 * dot(lookVector, spacecraftVelocity) / (slantRange * m_wavelength);
 
      return dopplerShift;
    };
@@ -386,13 +381,9 @@ double UsgsAstroSarSensorModel::dopplerShift(
 double UsgsAstroSarSensorModel::slantRange(csm::EcefCoord surfPt,
     double time) const
 {
+  csm::EcefVector surfVec(surfPt.x ,surfPt.y, surfPt.z);
   csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
-  double lookVector[3];
-
-  lookVector[0] = spacecraftPosition.x - surfPt.x;
-  lookVector[1] = spacecraftPosition.y - surfPt.y;
-  lookVector[2] = spacecraftPosition.z - surfPt.z;
-  return sqrt(pow(lookVector[0], 2) +  pow(lookVector[1], 2) + pow(lookVector[2], 2));
+  return norm(spacecraftPosition - surfVec);
 }
 
 double UsgsAstroSarSensorModel::slantRangeToGroundRange(
@@ -405,8 +396,8 @@ double UsgsAstroSarSensorModel::slantRangeToGroundRange(
 
   // Calculates the ground range from the slant range.
   std::function<double(double)> slantRangeToGroundRangeFunction =
-    [coeffs, slantRange](double groundRange){
-   return slantRange - (coeffs[0] + groundRange * (coeffs[1] + groundRange * (coeffs[2] + groundRange * coeffs[3])));
+    [this, coeffs, slantRange](double groundRange){
+   return slantRange - groundRangeToSlantRange(groundRange, coeffs);
   };
 
   // Need to come up with an initial guess when solving for ground
@@ -424,6 +415,11 @@ double UsgsAstroSarSensorModel::slantRangeToGroundRange(
   return brentRoot(minGroundRangeGuess, maxGroundRangeGuess, slantRangeToGroundRangeFunction, tolerance);
 }
 
+double UsgsAstroSarSensorModel::groundRangeToSlantRange(double groundRange, const std::vector<double> &coeffs) const
+{
+  return coeffs[0] + groundRange * (coeffs[1] + groundRange * (coeffs[2] + groundRange * coeffs[3]));
+}
+
 
 csm::ImageCoordCovar UsgsAstroSarSensorModel::groundToImage(
     const csm::EcefCoordCovar& groundPt,
@@ -443,7 +439,36 @@ csm::EcefCoord UsgsAstroSarSensorModel::imageToGround(
     double* achievedPrecision,
     csm::WarningList* warnings) const
 {
-  return csm::EcefCoord(0.0, 0.0, 0.0);
+  double time = m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration;
+  double groundRange = imagePt.samp * m_scaledPixelWidth;
+  std::vector<double> coeffs = getRangeCoefficients(time);
+  double slantRange = groundRangeToSlantRange(groundRange, coeffs);
+
+  // Compute the in-track, cross-track, nadir, coordinate system to solve in
+  csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
+  double positionMag = norm(spacecraftPosition);
+  csm::EcefVector spacecraftVelocity = getSpacecraftVelocity(time);
+  // In-track unit vector
+  csm::EcefVector vHat = normalized(spacecraftVelocity);
+  // Nadir unit vector
+  csm::EcefVector tHat = normalized(rejection(spacecraftPosition, vHat));
+  // Cross-track unit vector
+  csm::EcefVector uHat = cross(vHat, tHat);
+
+  // Compute the spacecraft position in the new coordinate system
+  //   The cross-track unit vector is orthogonal to the position so we ignore it
+  double nadirComp = dot(spacecraftPosition, tHat);
+  double inTrackComp = dot(spacecraftPosition, vHat);
+
+  // Compute the substituted values
+  // TODO properly handle ellipsoid radii
+  double radiusSqr = m_majorAxis * m_majorAxis;
+  double alpha = (radiusSqr - slantRange * slantRange - positionMag * positionMag) / (2 * nadirComp);
+  // TODO use right/left look to determine +/-
+  double beta = -sqrt(slantRange * slantRange - alpha * alpha);
+  csm::EcefVector groundVec = alpha * tHat + beta * uHat + spacecraftPosition;
+
+  return csm::EcefCoord(groundVec.x, groundVec.y, groundVec.z);
 }
 
 csm::EcefCoordCovar UsgsAstroSarSensorModel::imageToGround(
diff --git a/src/Utilities.cpp b/src/Utilities.cpp
index 08101a85b3b0f719e30631dcf039969706ac7908..38cbfa7cdbcbef78a8e48d3bb9c6a25990fe1102 100644
--- a/src/Utilities.cpp
+++ b/src/Utilities.cpp
@@ -405,6 +405,74 @@ double secantRoot(double lowerBound, double upperBound, std::function<double(dou
 }
 
 
+csm::EcefVector operator*(double scalar, const csm::EcefVector &vec)
+{
+  return csm::EcefVector(
+      scalar * vec.x,
+      scalar * vec.y,
+      scalar * vec.z);
+}
+
+csm::EcefVector operator*(const csm::EcefVector &vec, double scalar)
+{
+  return scalar * vec;
+}
+
+csm::EcefVector operator/(const csm::EcefVector &vec, double scalar)
+{
+  return 1.0 / scalar * vec;
+}
+
+csm::EcefVector operator+(const csm::EcefVector &vec1, const csm::EcefVector &vec2)
+{
+  return csm::EcefVector(
+      vec1.x + vec2.x,
+      vec1.y + vec2.y,
+      vec1.z + vec2.z);
+}
+
+csm::EcefVector operator-(const csm::EcefVector &vec1, const csm::EcefVector &vec2)
+{
+  return csm::EcefVector(
+      vec1.x - vec2.x,
+      vec1.y - vec2.y,
+      vec1.z - vec2.z);
+}
+
+double dot(const csm::EcefVector &vec1, const csm::EcefVector &vec2)
+{
+  return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z;
+}
+
+csm::EcefVector cross(const csm::EcefVector &vec1, const csm::EcefVector &vec2)
+{
+  return csm::EcefVector(
+      vec1.y * vec2.z - vec1.z * vec2.y,
+      vec1.z * vec2.x - vec1.x * vec2.z,
+      vec1.x * vec2.y - vec1.y * vec2.x);
+}
+
+double norm(const csm::EcefVector &vec)
+{
+  return sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
+}
+
+csm::EcefVector normalized(const csm::EcefVector &vec)
+{
+  return vec / norm(vec);
+}
+
+csm::EcefVector projection(const csm::EcefVector &vec1, const csm::EcefVector &vec2)
+{
+  return dot(vec1, vec2) / dot(vec2, vec2) * vec2;
+}
+
+csm::EcefVector rejection(const csm::EcefVector &vec1, const csm::EcefVector &vec2)
+{
+  return vec1 - projection(vec1, vec2);
+}
+
+
 // convert a measurement
 double metric_conversion(double val, std::string from, std::string to) {
     json typemap = {
diff --git a/tests/SarTests.cpp b/tests/SarTests.cpp
index 59cb3727297c9d69176436cb1311b30dec4ecf9e..5cff6a0e63666d5045f1eaed463af66d7729ad85 100644
--- a/tests/SarTests.cpp
+++ b/tests/SarTests.cpp
@@ -44,11 +44,12 @@ TEST_F(SarSensorModel, State) {
 }
 
 TEST_F(SarSensorModel, Center) {
-  csm::ImageCoord imagePt(7.5, 7.5);
+  csm::ImageCoord imagePt(500.0, 500.0);
   csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
-  EXPECT_NEAR(groundPt.x, 0, 1e-8);
-  EXPECT_NEAR(groundPt.y, 0, 1e-8);
-  EXPECT_NEAR(groundPt.z, 0, 1e-8);
+  // TODO these tolerances are bad
+  EXPECT_NEAR(groundPt.x, 1737391.90602155, 1e-2);
+  EXPECT_NEAR(groundPt.y, 3749.98835331, 1e-2);
+  EXPECT_NEAR(groundPt.z, -3749.99708833, 1e-2);
 }
 
 TEST_F(SarSensorModel, GroundToImage) {
diff --git a/tests/UtilitiesTests.cpp b/tests/UtilitiesTests.cpp
index 2400cb372e26edefc98a50b1418f81b831f0349d..ba98ca736c969b2544047a23dcb11c1cc3d6af21 100644
--- a/tests/UtilitiesTests.cpp
+++ b/tests/UtilitiesTests.cpp
@@ -358,3 +358,137 @@ TEST(UtilitiesTests, secantRoot) {
   EXPECT_NEAR(secantRoot(0.0, -3.0, testPoly, 1e-10), -1.0, 1e-10);
   EXPECT_THROW(secantRoot(-1000.0, 1000.0, testPoly), csm::Error);
 }
+
+TEST(UtilitiesTests, vectorProduct) {
+  csm::EcefVector vec(1.0, 2.0, 3.0);
+  csm::EcefVector leftVec = 2.0 * vec;
+  csm::EcefVector rightVec = vec * 2;
+  EXPECT_DOUBLE_EQ(leftVec.x, 2.0);
+  EXPECT_DOUBLE_EQ(leftVec.y, 4.0);
+  EXPECT_DOUBLE_EQ(leftVec.z, 6.0);
+  EXPECT_DOUBLE_EQ(rightVec.x, 2.0);
+  EXPECT_DOUBLE_EQ(rightVec.y, 4.0);
+  EXPECT_DOUBLE_EQ(rightVec.z, 6.0);
+}
+
+TEST(UtilitiesTests, vectorDivision) {
+  csm::EcefVector vec(2.0, 4.0, 6.0);
+  csm::EcefVector divVec = vec / 2.0;
+  EXPECT_DOUBLE_EQ(divVec.x, 1.0);
+  EXPECT_DOUBLE_EQ(divVec.y, 2.0);
+  EXPECT_DOUBLE_EQ(divVec.z, 3.0);
+}
+
+TEST(UtilitiesTests, vectorAddition) {
+  csm::EcefVector vec1(2.0, 4.0, 6.0);
+  csm::EcefVector vec2(1.0, 2.0, 3.0);
+  csm::EcefVector sumVec = vec1 + vec2;
+  EXPECT_DOUBLE_EQ(sumVec.x, 3.0);
+  EXPECT_DOUBLE_EQ(sumVec.y, 6.0);
+  EXPECT_DOUBLE_EQ(sumVec.z, 9.0);
+}
+
+TEST(UtilitiesTests, vectorSubtraction) {
+  csm::EcefVector vec1(2.0, 4.0, 6.0);
+  csm::EcefVector vec2(1.0, 2.0, 3.0);
+  csm::EcefVector diffVec = vec1 - vec2;
+  EXPECT_DOUBLE_EQ(diffVec.x, 1.0);
+  EXPECT_DOUBLE_EQ(diffVec.y, 2.0);
+  EXPECT_DOUBLE_EQ(diffVec.z, 3.0);
+}
+
+TEST(UtilitiesTests, vectorDot) {
+  csm::EcefVector unitX(1.0, 0.0, 0.0);
+  csm::EcefVector unitY(0.0, 1.0, 0.0);
+  csm::EcefVector unitZ(0.0, 0.0, 1.0);
+  csm::EcefVector testVec(1.0, 2.0, 3.0);
+  EXPECT_DOUBLE_EQ(dot(testVec, unitX), 1.0);
+  EXPECT_DOUBLE_EQ(dot(testVec, unitY), 2.0);
+  EXPECT_DOUBLE_EQ(dot(testVec, unitZ), 3.0);
+}
+
+TEST(UtilitiesTests, vectorCross) {
+  csm::EcefVector unitX(1.0, 0.0, 0.0);
+  csm::EcefVector unitY(0.0, 1.0, 0.0);
+  csm::EcefVector unitZ(0.0, 0.0, 1.0);
+
+  csm::EcefVector unitXY = cross(unitX, unitY);
+  EXPECT_DOUBLE_EQ(unitXY.x, 0.0);
+  EXPECT_DOUBLE_EQ(unitXY.y, 0.0);
+  EXPECT_DOUBLE_EQ(unitXY.z, 1.0);
+
+
+  csm::EcefVector unitYX = cross(unitY, unitX);
+  EXPECT_DOUBLE_EQ(unitYX.x, 0.0);
+  EXPECT_DOUBLE_EQ(unitYX.y, 0.0);
+  EXPECT_DOUBLE_EQ(unitYX.z, -1.0);
+
+  csm::EcefVector unitXZ = cross(unitX, unitZ);
+  EXPECT_DOUBLE_EQ(unitXZ.x, 0.0);
+  EXPECT_DOUBLE_EQ(unitXZ.y, -1.0);
+  EXPECT_DOUBLE_EQ(unitXZ.z, 0.0);
+
+
+  csm::EcefVector unitZX = cross(unitZ, unitX);
+  EXPECT_DOUBLE_EQ(unitZX.x, 0.0);
+  EXPECT_DOUBLE_EQ(unitZX.y, 1.0);
+  EXPECT_DOUBLE_EQ(unitZX.z, 0.0);
+
+  csm::EcefVector unitYZ = cross(unitY, unitZ);
+  EXPECT_DOUBLE_EQ(unitYZ.x, 1.0);
+  EXPECT_DOUBLE_EQ(unitYZ.y, 0.0);
+  EXPECT_DOUBLE_EQ(unitYZ.z, 0.0);
+
+
+  csm::EcefVector unitZY = cross(unitZ, unitY);
+  EXPECT_DOUBLE_EQ(unitZY.x, -1.0);
+  EXPECT_DOUBLE_EQ(unitZY.y, 0.0);
+  EXPECT_DOUBLE_EQ(unitZY.z, 0.0);
+}
+
+TEST(UtilitiesTests, vectorNorm) {
+  csm::EcefVector testVec(1.0, 2.0, 3.0);
+  EXPECT_DOUBLE_EQ(norm(testVec), sqrt(14.0));
+  csm::EcefVector normVec = normalized(testVec);
+  EXPECT_DOUBLE_EQ(normVec.x, 1.0 / sqrt(14.0));
+  EXPECT_DOUBLE_EQ(normVec.y, 2.0 / sqrt(14.0));
+  EXPECT_DOUBLE_EQ(normVec.z, 3.0 / sqrt(14.0));
+}
+
+TEST(UtilitiesTests, vectorProjection) {
+  csm::EcefVector unitX(1.0, 0.0, 0.0);
+  csm::EcefVector unitY(0.0, 1.0, 0.0);
+  csm::EcefVector unitZ(0.0, 0.0, 1.0);
+
+  csm::EcefVector testVec(1.0, 2.0, 3.0);
+
+  csm::EcefVector projX = projection(testVec, unitX);
+  EXPECT_DOUBLE_EQ(projX.x, 1.0);
+  EXPECT_DOUBLE_EQ(projX.y, 0.0);
+  EXPECT_DOUBLE_EQ(projX.z, 0.0);
+
+  csm::EcefVector rejectX = rejection(testVec, unitX);
+  EXPECT_DOUBLE_EQ(rejectX.x, 0.0);
+  EXPECT_DOUBLE_EQ(rejectX.y, 2.0);
+  EXPECT_DOUBLE_EQ(rejectX.z, 3.0);
+
+  csm::EcefVector projY = projection(testVec, unitY);
+  EXPECT_DOUBLE_EQ(projY.x, 0.0);
+  EXPECT_DOUBLE_EQ(projY.y, 2.0);
+  EXPECT_DOUBLE_EQ(projY.z, 0.0);
+
+  csm::EcefVector rejectY = rejection(testVec, unitY);
+  EXPECT_DOUBLE_EQ(rejectY.x, 1.0);
+  EXPECT_DOUBLE_EQ(rejectY.y, 0.0);
+  EXPECT_DOUBLE_EQ(rejectY.z, 3.0);
+
+  csm::EcefVector projZ = projection(testVec, unitZ);
+  EXPECT_DOUBLE_EQ(projZ.x, 0.0);
+  EXPECT_DOUBLE_EQ(projZ.y, 0.0);
+  EXPECT_DOUBLE_EQ(projZ.z, 3.0);
+
+  csm::EcefVector rejectZ = rejection(testVec, unitZ);
+  EXPECT_DOUBLE_EQ(rejectZ.x, 1.0);
+  EXPECT_DOUBLE_EQ(rejectZ.y, 2.0);
+  EXPECT_DOUBLE_EQ(rejectZ.z, 0.0);
+}