diff --git a/src/transmissionReconstruction/dataFilter/sensitivityCalculations.cpp b/src/transmissionReconstruction/dataFilter/sensitivityCalculations.cpp
new file mode 100644
index 0000000..69ef2d9
--- /dev/null
+++ b/src/transmissionReconstruction/dataFilter/sensitivityCalculations.cpp
@@ -0,0 +1,224 @@
+#include "sensitivityCalculations.h"
+#include "Function1D.h"
+#include "Function2D.h"
+#include "Function3D.h"
+
+#include <cmath>
+#include <cstddef>
+#include <iostream>
+#include <omp.h>
+
+namespace {
+inline void cross(double *aVA, double *aVB, double *aVOut, int aIncX = 1) {
+  if (!aVA || !aVB || !aVOut)
+    return;
+  aVOut[0 * aIncX] =
+      (aVA[1 * aIncX] * aVB[2 * aIncX] - aVA[2 * aIncX] * aVB[1 * aIncX]);
+  aVOut[1 * aIncX] =
+      (aVA[2 * aIncX] * aVB[0 * aIncX] - aVA[0 * aIncX] * aVB[2 * aIncX]);
+  aVOut[2 * aIncX] =
+      (aVA[0 * aIncX] * aVB[1 * aIncX] - aVA[1 * aIncX] * aVB[0 * aIncX]);
+}
+inline Aurora::Matrix cross(const Aurora::Matrix &aMA,
+                            const Aurora::Matrix &aMB) {
+  if (!aMA.compareShape(aMB)) {
+    std::cerr << "cross must with same shape Matrix!" << std::endl;
+    return Aurora::Matrix();
+  }
+  if (aMA.getDimSize(0) != 3 && aMA.getDimSize(1) != 3) {
+    std::cerr << "cross must with Matrix(3,N) or Matrix(N,3)!" << std::endl;
+    return Aurora::Matrix();
+  }
+  auto output = Aurora::zeros(aMA.getDimSize(0), aMA.getDimSize(1));
+  if (aMA.getDimSize(0) == 3) {
+    for (size_t i = 0; i < aMA.getDimSize(1); ++i) {
+      cross(aMA.getData() + i * 3, aMB.getData() + i * 3,
+            output.getData() + i * 3);
+    }
+  } else {
+    for (size_t i = 0; i < aMA.getDimSize(0); ++i) {
+      cross(aMA.getData() + i, aMB.getData() + i, output.getData() + i,
+            aMA.getDimSize(0));
+    }
+  }
+
+  return output;
+}
+inline Aurora::Matrix getVector(const Aurora::Matrix &aM1,
+                                const Aurora::Matrix &aM2) {
+  auto temp = ::cross(Aurora::transpose(aM1), aM2);
+  temp = temp / Aurora::repmat(Aurora::sqrt(Aurora::sum(temp ^ 2, Aurora::Row)),
+                               1, temp.getDimSize(1));
+  return temp;
+}
+
+inline Aurora::Matrix sliceColumnsFrom4D(const Aurora::Matrix &aMSrc,
+                                         const Aurora::Matrix &aVCols,
+                                         const Aurora::Matrix &aVSlices) {
+  auto output = Aurora::zeros(aMSrc.getDimSize(0), aVCols.getDataSize(),
+                              aVSlices.getDataSize());
+  for (size_t i = 0; i < aVSlices.getDataSize(); i++) {
+    size_t sliceIdx = aVSlices.getData()[i];
+    for (size_t j = 0; j < aVCols.getDataSize(); j++) {
+      size_t colIdx = aVCols.getData()[j];
+      output(Aurora::$, j, i) = aMSrc(Aurora::$, colIdx - 1, sliceIdx - 1);
+    }
+  }
+  return output;
+}
+
+#define PI 3.141592653589793238462
+} // namespace
+
+namespace Recon {
+Aurora::Matrix getSensitivity(const Aurora::Matrix &aMSensmap,
+                              Aurora::Matrix aVSenderNormal,
+                              const Aurora::Matrix &aMDirToReceiver) {
+  aVSenderNormal.forceReshape(aVSenderNormal.getDataSize(), 1, 1);
+  auto dir = aMDirToReceiver;
+  dir = dir / Aurora::repmat(Aurora::sqrt(Aurora::sum(dir ^ 2)),
+                             dir.getDimSize(0), 1);
+  aVSenderNormal = Aurora::repmat(aVSenderNormal, 1, dir.getDimSize(1));
+  auto xyn = Aurora::zeros(1, 3);
+  xyn.getData()[2] = 1;
+  xyn = Aurora::repmat(xyn, dir.getDimSize(1), 1);
+  auto xyd = ::getVector(::cross(Aurora::transpose(xyn), dir), xyn);
+  auto xysn = ::getVector(::cross(Aurora::transpose(xyn), aVSenderNormal), xyn);
+  auto xzn = ::getVector(aVSenderNormal, xyn);
+  auto xzd = ::getVector(::cross(Aurora::transpose(xzn), dir), xzn);
+  auto xzsn = ::getVector(::cross(Aurora::transpose(xzn), aVSenderNormal), xzn);
+  auto xi = Aurora::round(Aurora::acos(Aurora::sum(xyd * xysn, Aurora::Row)) *
+                          180 / PI);
+  auto zi = Aurora::round(Aurora::acos(Aurora::sum(xzd * xzsn, Aurora::Row)) *
+                          180 / PI);
+  for (size_t i = 0; i < xi.getDataSize(); ++i) {
+    if (xi.getData()[i] > 90)
+      xi.getData()[i] -= 90;
+    else if (xi.getData()[i] < 0)
+      xi.getData()[i] += 90;
+    if (zi.getData()[i] > 90)
+      zi.getData()[i] -= 90;
+    else if (zi.getData()[i] < 0)
+      zi.getData()[i] += 90;
+  }
+  double msensx = std::round(((double)aMSensmap.getDimSize(0) - 1.0) * 0.5);
+  double msensY = std::round(((double)aMSensmap.getDimSize(1) - 1.0) * 0.5);
+  xi = xi + 1 + msensx;
+  zi = zi + 1 + msensY;
+  for (size_t i = 0; i < xi.getDataSize(); ++i) {
+    if (xi.getData()[i] < 1) {
+      xi.getData()[i] = 1;
+    } else if (xi.getData()[i] > 181) {
+      xi.getData()[i] = 181;
+    }
+    if (zi.getData()[i] > 181) {
+      zi.getData()[i] = 181;
+    } else if (zi.getData()[i] < 1) {
+      zi.getData()[i] = 1;
+    }
+  }
+  Aurora::nantoval(xi, 1.0);
+  Aurora::nantoval(zi, 1.0);
+  auto idx = Aurora::sub2ind(Aurora::size(aMSensmap), {xi, zi});
+  auto out = Aurora::zeros(idx.getDataSize(), 1, 1);
+  for (size_t i = 0; i < idx.getDataSize(); ++i) {
+    out.getData()[i] = aMSensmap.getData()[(int)idx.getData()[i] - 1];
+  }
+  return out;
+}
+
+Aurora::Matrix precalcSensitivityForTAS(const Aurora::Matrix &aMSensChar,
+                                        const Aurora::Matrix &aMStartPositions,
+                                        const Aurora::Matrix &aMStartNormals,
+                                        const Aurora::Matrix &aMEndPositions) {
+  auto sens = Aurora::zeros(aMEndPositions.getDimSize(1),
+                            aMStartPositions.getDimSize(1));
+  for (size_t i = 0; i < aMStartPositions.getDimSize(1); ++i) {
+    auto dirVector = aMEndPositions -
+                     Aurora::repmat(aMStartPositions(Aurora::$, i).toMatrix(),
+                                    1, aMEndPositions.getDimSize(1));
+    sens(Aurora::$, i) = getSensitivity(
+        aMSensChar, aMStartNormals(Aurora::$, i).toMatrix(), dirVector);
+  }
+  return sens;
+}
+
+std::vector<Aurora::Matrix>
+combineSensitivity(const Aurora::Matrix &aVSenderTASRange,
+                   const Aurora::Matrix &aVSenderElementRange,
+                   const Aurora::Matrix &aVReceiverTASRange,
+                   const Aurora::Matrix &aVReceiverElementRange,
+                   const Aurora::Matrix &aMSenderSens,
+                   const Aurora::Matrix &aMReceiverSens) {
+  double maxReceiverElementRange =
+      Aurora::max(aVReceiverElementRange).getScalar();
+  double maxReceiverTASRange = Aurora::max(aVReceiverTASRange).getScalar();
+  double maxSenderElementRange = Aurora::max(aVSenderElementRange).getScalar();
+  double maxSenderTASRange = Aurora::max(aVSenderTASRange).getScalar();
+  std::vector<Aurora::Matrix> ret;
+  for (size_t i = 0; i < maxSenderTASRange; ++i) {
+    ret.emplace_back(Aurora::zeros(maxReceiverElementRange, maxReceiverTASRange,
+                                   maxSenderElementRange));
+  }
+  size_t countSE = 0;
+  for (size_t i = 0; i < aVSenderTASRange.getDataSize(); i++) {
+    auto se = aVReceiverTASRange.getData()[i];
+    for (size_t j = 0; j < aVSenderElementRange.getDataSize(); j++) {
+      auto sn = aVSenderElementRange.getData()[j];
+      size_t countRE = 0;
+      for (size_t k = 0; k < aVReceiverTASRange.getDataSize(); k++) {
+        auto re = aVReceiverTASRange.getData()[k];
+        for (size_t n = 0; n < aVReceiverElementRange.getDataSize(); n++) {
+          auto rn = aVReceiverElementRange.getData()[n];
+          ret[se - 1](rn - 1, re - 1, sn - 1) =
+              aMSenderSens(countRE, countSE).toMatrix().getScalar() *
+              aMReceiverSens(countSE, countRE).toMatrix().getScalar();
+          countRE++;
+        }
+      }
+      countSE++;
+    }
+  }
+  return ret;
+}
+
+std::vector<Aurora::Matrix>
+precalcSensitivity(const Aurora::Matrix &aVSenderTASRange,
+                   const Aurora::Matrix &aVSenderElementRange,
+                   const Aurora::Matrix &aVReceiverTASRange,
+                   const Aurora::Matrix &aVReceiverElementRange,
+                   const Aurora::Matrix &aVMotorPos, const geo &aTASElements) {
+  size_t dim = aTASElements.receiverPositions[0].getDimSize(0);
+  size_t pos1Idx = (int)aVMotorPos.getData()[0] - 1;
+  auto receiverPositions = aTASElements.receiverPositions[pos1Idx];
+  receiverPositions = ::sliceColumnsFrom4D(
+      receiverPositions, aVReceiverElementRange, aVReceiverTASRange);
+  receiverPositions.forceReshape(dim, receiverPositions.getDataSize() / dim, 1);
+
+  auto receiverNormals = aTASElements.receiverNormals[pos1Idx];
+  receiverNormals = ::sliceColumnsFrom4D(
+      receiverNormals, aVReceiverElementRange, aVReceiverTASRange);
+  receiverNormals.forceReshape(dim, receiverNormals.getDataSize() / dim, 1);
+
+  auto senderPositions = aTASElements.senderPositions[pos1Idx];
+  senderPositions = ::sliceColumnsFrom4D(senderPositions, aVSenderElementRange,
+                                         aVSenderTASRange);
+  senderPositions.forceReshape(dim, senderPositions.getDataSize() / dim, 1);
+
+  auto senderNormals = aTASElements.senderNormals[pos1Idx];
+  senderNormals = ::sliceColumnsFrom4D(senderNormals, aVSenderElementRange,
+                                       aVSenderTASRange);
+  senderNormals.forceReshape(dim, senderNormals.getDataSize() / dim, 1);
+
+  auto senderSens = precalcSensitivityForTAS(
+      aTASElements.sensChar, senderPositions, senderNormals, receiverPositions);
+  auto receiverSens =
+      precalcSensitivityForTAS(aTASElements.sensChar, receiverPositions,
+                               receiverNormals, senderPositions);
+
+  auto sensPrecalc = combineSensitivity(
+      aVSenderTASRange, aVSenderElementRange, aVReceiverTASRange,
+      aVReceiverElementRange, senderSens, receiverSens);
+  return sensPrecalc;
+}
+} // namespace Recon
\ No newline at end of file
diff --git a/src/transmissionReconstruction/dataFilter/sensitivityCalculations.h b/src/transmissionReconstruction/dataFilter/sensitivityCalculations.h
new file mode 100644
index 0000000..c3eda32
--- /dev/null
+++ b/src/transmissionReconstruction/dataFilter/sensitivityCalculations.h
@@ -0,0 +1,44 @@
+#ifndef __SENSITIVITYCALCULATIONS_H__
+#define __SENSITIVITYCALCULATIONS_H__
+#include <vector>
+
+#include "Matrix.h"
+
+namespace Recon {
+struct geo {
+  //[471][4]
+  Aurora::Matrix headTable;
+  //[181][181]
+  Aurora::Matrix sensChar;
+  std::vector<Aurora::Matrix> senderNormals;
+  std::vector<Aurora::Matrix> receiverNormals;
+  std::vector<Aurora::Matrix> senderPositions;
+  std::vector<Aurora::Matrix> receiverPositions;
+};
+
+Aurora::Matrix getSensitivity(const Aurora::Matrix &aMSensmap,
+                              Aurora::Matrix aVSenderNormal,
+                              const Aurora::Matrix &aMDirToReceiver);
+
+Aurora::Matrix precalcSensitivityForTAS(const Aurora::Matrix &aMSensChar,
+                                        const Aurora::Matrix &aMStartPositions,
+                                        const Aurora::Matrix &aMStartNormals,
+                                        const Aurora::Matrix &aMEndPositions);
+
+std::vector<Aurora::Matrix>
+combineSensitivity(const Aurora::Matrix &aVSenderTASRange,
+                   const Aurora::Matrix &aVSenderElementRange,
+                   const Aurora::Matrix &aVReceiverTASRange,
+                   const Aurora::Matrix &aVReceiverElementRange,
+                   const Aurora::Matrix &aMSenderSens,
+                   const Aurora::Matrix &aMReceiverSens);
+
+std::vector<Aurora::Matrix>
+precalcSensitivity(const Aurora::Matrix &aVSenderTASRange,
+                   const Aurora::Matrix &aVSenderElementRange,
+                   const Aurora::Matrix &aVReceiverTASRange,
+                   const Aurora::Matrix &aVReceiverElementRange,
+                   const Aurora::Matrix &aVMotorPos,
+                   const geo &aTASElements);
+} // namespace Recon
+#endif // __SENSITIVITYCALCULATIONS_H__
\ No newline at end of file
diff --git a/src/transmissionReconstruction/dataPreperation.h b/src/transmissionReconstruction/dataPreperation.h
index 25ac5b3..c5da1c0 100644
--- a/src/transmissionReconstruction/dataPreperation.h
+++ b/src/transmissionReconstruction/dataPreperation.h
@@ -3,8 +3,8 @@
 #include "Matrix.h"
 namespace Recon {
 
-Aurora::Matrix distanceBetweenTwoPoints(Aurora::Matrix aPtsA,
-                                        Aurora::Matrix aPtsB);
+Aurora::Matrix distanceBetweenTwoPoints(Aurora::Matrix aMPtsA,
+                                        Aurora::Matrix aMPtsB);
 
 Aurora::Matrix calculateWaterTemperature(Aurora::Matrix aMWaterTempS,
                                          Aurora::Matrix aMWaterTempR,
@@ -12,7 +12,7 @@ Aurora::Matrix calculateWaterTemperature(Aurora::Matrix aMWaterTempS,
                                          Aurora::Matrix aVrl,
                                          Aurora::Matrix aVmp);
                                          
-Aurora::Matrix prepareAScansForTransmissionDetection(const Aurora::Matrix& aMaScanBlock, const Aurora::Matrix aVGainBlock);
+Aurora::Matrix prepareAScansForTransmissionDetection(const Aurora::Matrix& aMAScanBlock, const Aurora::Matrix aVGainBlock);
 } // namespace Recon
 
 #endif // __DATAPREPERATION_H__
\ No newline at end of file
diff --git a/test/Sensitivity_Test.cpp b/test/Sensitivity_Test.cpp
new file mode 100644
index 0000000..b98f9af
--- /dev/null
+++ b/test/Sensitivity_Test.cpp
@@ -0,0 +1,126 @@
+#include <gtest/gtest.h>
+
+#include "Matrix.h"
+#include "transmissionReconstruction/dataFilter/sensitivityCalculations.h"
+#include "MatlabReader.h"
+
+inline double fourDecimalRound(double src){
+    return round(src*10000.0)/10000.0;
+}
+
+#define EXPECT_DOUBLE_AE(valueA,valueB)\
+    EXPECT_DOUBLE_EQ(fourDecimalRound(valueA),fourDecimalRound(valueB))
+
+#define ASSERT_DOUBLE_AE(valueA,valueB)\
+    ASSERT_DOUBLE_EQ(fourDecimalRound(valueA),fourDecimalRound(valueB))
+
+class Sensitivity_Test : public ::testing::Test {
+protected:
+    static void SetUpSensitivityTester() {
+
+    }
+
+    static void TearDownTestCase() {
+    }
+
+    void SetUp() {
+    }
+
+    void TearDown() {
+    }
+};
+
+TEST_F(Sensitivity_Test, getSensitivity) {
+    MatlabReader m("/home/krad/TestData/sensitivity.mat");
+
+    auto sensmap = m.read("sensmap");
+    auto senderNormal = m.read("senderNormal");
+    auto dirToReceiver = m.read("dirVector");
+
+    auto output = Recon::getSensitivity(sensmap, senderNormal, dirToReceiver);
+
+    auto sens = m.read("sens");
+    EXPECT_EQ(output.getDataSize(), sens.getDataSize());
+    for (size_t i = 0; i < output.getDataSize(); i++)
+    {
+        EXPECT_DOUBLE_AE(output.getData()[i],sens.getData()[i])<<", index:"<<i;
+    }
+    
+}
+
+TEST_F(Sensitivity_Test, precalcSensitivityForTAS){
+    MatlabReader m("/home/krad/TestData/sensTAS.mat");
+
+    auto sensmap = m.read("sensChar");
+    auto senderNormals = m.read("senderNormals");
+    auto senderPositions = m.read("senderPositions");
+    auto receiverPositions = m.read("receiverPositions");
+
+
+    auto output = Recon::precalcSensitivityForTAS(sensmap, senderPositions,senderNormals, receiverPositions);
+
+    auto sens = m.read("TASResult");
+    EXPECT_EQ(output.getDataSize(), sens.getDataSize());
+    for (size_t i = 0; i < output.getDataSize(); i++)
+    {
+        EXPECT_DOUBLE_AE(output.getData()[i],sens.getData()[i])<<", index:"<<i;
+    }
+}
+
+TEST_F(Sensitivity_Test, combineSensitivity){
+    MatlabReader m("/home/krad/TestData/combine.mat");
+
+    auto senderTASRange = m.read("senderTASRange");
+    auto senderElementRange = m.read("senderElementRange");
+    auto receiverTASRange = m.read("receiverTASRange");
+    auto receiverElementRange = m.read("receiverElementRange");
+    auto senderSens = m.read("TASResult");
+    auto receiverSens = m.read("TASResult2");
+
+
+    auto output = Recon::combineSensitivity(senderTASRange, senderElementRange, receiverTASRange, receiverElementRange, senderSens, receiverSens);
+
+    auto sens = m.read4d("combineResult");
+    EXPECT_EQ(output.size(), sens.size());
+    EXPECT_EQ(output[0].getDataSize(), sens[0].getDataSize());
+
+    for (size_t i = 0; i < output.size(); i++)
+    {
+        for (size_t j = 0; j < output[i].getDataSize(); j++)
+        {
+            EXPECT_DOUBLE_AE(output[i].getData()[j],sens[i].getData()[j])<<"Matrix index:"<<i<<", value index:"<<j;
+            
+        }
+    }
+}
+
+TEST_F(Sensitivity_Test,precalcSensitivity){
+    MatlabReader m("/home/krad/TestData/precalcSensitivity.mat");
+
+    auto senderTASRange = m.read("senderTASRange");
+    auto senderElementRange = m.read("senderElementRange");
+    auto receiverTASRange = m.read("receiverTASRange");
+    auto receiverElementRange = m.read("receiverElementRange");
+    MatlabReader m3("/home/krad/TestData/precalcSensitivity2.mat");
+    Recon::geo TASElements;
+    TASElements.senderPositions = m3.read4d("senderPositions");
+    TASElements.senderNormals = m3.read4d("senderNormals");
+    TASElements.receiverPositions = m3.read4d("receiverPositions");
+    TASElements.receiverNormals = m3.read4d("receiverNormals");
+    TASElements.sensChar = m.read("sensChar");
+    auto motorPos = Aurora::Matrix::fromRawData(new double[2]{1,2}, 2);
+    auto output = precalcSensitivity(senderTASRange, senderElementRange, receiverTASRange, receiverElementRange, motorPos, TASElements);
+    MatlabReader m2("/home/krad/TestData/precalcResult.mat");
+    auto sens = m2.read4d("result");
+    EXPECT_EQ(output.size(), sens.size());
+    EXPECT_EQ(output[0].getDataSize(), sens[0].getDataSize());
+
+    for (size_t i = 0; i < output.size(); i++)
+    {
+        for (size_t j = 0; j < output[i].getDataSize(); j++)
+        {
+            ASSERT_DOUBLE_AE(output[i].getData()[j],sens[i].getData()[j])<<"Matrix index:"<<i<<", value index:"<<j;
+            
+        }
+    }
+}
\ No newline at end of file