diff --git a/Modules/IGT/Testing/mitkPolhemusTrackingDeviceHardwareTest.cpp b/Modules/IGT/Testing/mitkPolhemusTrackingDeviceHardwareTest.cpp
index 3513860fc5..5538870444 100644
--- a/Modules/IGT/Testing/mitkPolhemusTrackingDeviceHardwareTest.cpp
+++ b/Modules/IGT/Testing/mitkPolhemusTrackingDeviceHardwareTest.cpp
@@ -1,36 +1,65 @@
 /*===================================================================
 
 The Medical Imaging Interaction Toolkit (MITK)
 
 Copyright (c) German Cancer Research Center,
 Division of Medical and Biological Informatics.
 All rights reserved.
 
 This software is distributed WITHOUT ANY WARRANTY; without
 even the implied warranty of MERCHANTABILITY or FITNESS FOR
 A PARTICULAR PURPOSE.
 
 See LICENSE.txt or http://www.mitk.org for details.
 
 ===================================================================*/
 
 #include "mitkPolhemusInterface.h"
 #include "mitkTestingMacros.h"
 #include "mitkStandardFileLocations.h"
 #include <mitkIGTConfig.h>
 
-class mitkPolhemusTrackingDeviceHardwareTestClass
+// Testing
+#include "mitkTestingMacros.h"
+#include "mitkTestFixture.h"
+
+class mitkPolhemusTrackingDeviceHardwareTestSuite : public mitk::TestFixture
 {
+  CPPUNIT_TEST_SUITE(mitkPolhemusTrackingDeviceHardwareTestSuite);
+  // Test the append method
+  MITK_TEST(testInterface);
+  CPPUNIT_TEST_SUITE_END();
+
+
 public:
-};
+  void setUp() override
+  {
 
-/** This function is testing the Class PolhemusTrackingDevice in interaction with the hardware.
-  * So a MicronTracker Tracking System has to be installed and connected to run this test.
-  * The test needs the filenames of three toolfiles as arguments.
-  */
-int mitkPolhemusTrackingDeviceHardwareTest(int argc, char* argv[])
-{
-  MITK_TEST_BEGIN("PolhemusTrackingDeviceHardware");
+  }
+  void tearDown() override
+  {
+
+  }
+
+  void testInterface()
+  {
+    mitk::PolhemusInterface::Pointer myInterface = mitk::PolhemusInterface::New();
+    CPPUNIT_ASSERT_MESSAGE("Testing connection.", myInterface->Connect());
+    CPPUNIT_ASSERT_MESSAGE("Start tracking.", myInterface->StartTracking());
+
+    CPPUNIT_ASSERT_MESSAGE("Tracking 20 frames ...", true);
+    for (int i = 0; i < 20; i++)
+    {
+      std::vector<mitk::PolhemusInterface::trackingData> lastFrame = myInterface->GetLastFrame();
+      MITK_INFO << "Frame " << i;
+      for (int j = 0; j < lastFrame.size(); j++)
+      {
+        MITK_INFO << "[" << j << "]" << " Pos:" << lastFrame.at(j).pos << " Rot:" << lastFrame.at(j).rot;
+      }
+    }
+
+  }
+
+};
 
-  MITK_TEST_END();
-}
+MITK_TEST_SUITE_REGISTRATION(mitkPolhemusTrackingDeviceHardware)
diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp
index 2ac6d78335..2e80ddb018 100644
--- a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp
+++ b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp
@@ -1,46 +1,111 @@
 /*===================================================================
 
 The Medical Imaging Interaction Toolkit (MITK)
 
 Copyright (c) German Cancer Research Center,
 Division of Medical and Biological Informatics.
 All rights reserved.
 
 This software is distributed WITHOUT ANY WARRANTY; without
 even the implied warranty of MERCHANTABILITY or FITNESS FOR
 A PARTICULAR PURPOSE.
 
 See LICENSE.txt or http://www.mitk.org for details.
 
 ===================================================================*/
 
 #include <mitkPolhemusInterface.h>
 
-#include <windows.h>
-#include <tchar.h>
-#include <string>
-#include <PDI.h>
-
+BYTE  MotionBuf[0x1FA400];
 
 mitk::PolhemusInterface::PolhemusInterface()
 {
-
+  m_pdiDev.SetPnoBuffer(MotionBuf, 0x1FA400);
 }
 
 mitk::PolhemusInterface::~PolhemusInterface()
 {
 
 }
 
 bool mitk::PolhemusInterface::StartTracking()
 {
-
+    //return m_pdiDev.StartContPno(0);
   return true;
 }
 
 bool mitk::PolhemusInterface::StopTracking()
 {
 
   return true;
 
 }
+
+bool mitk::PolhemusInterface::Connect()
+{
+  m_pdiDev.Trace(TRUE, 7);
+  if (m_pdiDev.CnxReady()) { return true; }
+
+  ePiCommType eType = m_pdiDev.DiscoverCnx();
+  switch (eType)
+  {
+  case PI_CNX_USB:
+    MITK_INFO << "USB Connection: " << m_pdiDev.GetLastResultStr();
+    break;
+  case PI_CNX_SERIAL:
+    MITK_INFO << "Serial Connection: " << m_pdiDev.GetLastResultStr();
+    break;
+  default:
+    MITK_INFO << "DiscoverCnx result: " << m_pdiDev.GetLastResultStr();
+    break;
+  }
+
+  return m_pdiDev.CnxReady();
+}
+
+bool mitk::PolhemusInterface::Disconnect()
+{
+  m_pdiDev.Disconnect();
+  return false;
+}
+
+std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::GetLastFrame()
+{
+  PBYTE pBuf;
+  DWORD dwSize;
+
+  //read one frame
+  if (!m_pdiDev.ReadSinglePnoBuf(pBuf, dwSize)) {MITK_WARN << m_pdiDev.GetLastResultStr();}
+
+  std::vector<mitk::PolhemusInterface::trackingData> returnValue;
+
+  DWORD i = 0;
+
+  while (i<dwSize)
+  {
+    BYTE ucSensor = pBuf[i + 2];
+    SHORT shSize = pBuf[i + 6];
+
+    // skip rest of header
+    i += 8;
+
+    PDWORD pFC = (PDWORD)(&pBuf[i]);
+    PFLOAT pPno = (PFLOAT)(&pBuf[i + 4]);
+
+    mitk::PolhemusInterface::trackingData currentTrackingData;
+
+    currentTrackingData.pos[0] = pPno[0];
+    currentTrackingData.pos[1] = pPno[1];
+    currentTrackingData.pos[2] = pPno[2];
+
+    double azimuthAngle = pPno[3];
+    double elevationAngle = pPno[4];
+    double rollAngle = pPno[5];
+    vnl_quaternion<double> eulerQuat(rollAngle, elevationAngle, azimuthAngle);
+        currentTrackingData.rot = eulerQuat;
+
+    returnValue.push_back(currentTrackingData);
+    i += shSize;
+  }
+  return returnValue;
+}
diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h
index fa834c7996..d0ccff6e9f 100644
--- a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h
+++ b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h
@@ -1,72 +1,99 @@
 /*===================================================================
 
 The Medical Imaging Interaction Toolkit (MITK)
 
 Copyright (c) German Cancer Research Center,
 Division of Medical and Biological Informatics.
 All rights reserved.
 
 This software is distributed WITHOUT ANY WARRANTY; without
 even the implied warranty of MERCHANTABILITY or FITNESS FOR
 A PARTICULAR PURPOSE.
 
 See LICENSE.txt or http://www.mitk.org for details.
 
 ===================================================================*/
 
 #ifndef MITKPolhemusINTERFACE_H_HEADER_INCLUDED_
 #define MITKPolhemusINTERFACE_H_HEADER_INCLUDED_
 
 
 #include <vector>
 #include <string>
 
 #include <MitkIGTExports.h>
 #include "mitkCommon.h"
 
 #include <itkObject.h>
 #include <itkObjectFactory.h>
 
+#include <windows.h>
+#include <tchar.h>
+#include <string>
+#include <PDI.h>
+
+#include <mitkNavigationData.h>
+
 
 
 namespace mitk
 {
 
   /** Documentation:
   *   \brief An object of this class represents the interface to Polhemus trackers.
   *   \ingroup IGT
   */
   class MITKIGT_EXPORT PolhemusInterface : public itk::Object
   {
   public:
 
     mitkClassMacroItkParent(PolhemusInterface,itk::Object);
-    itkFactorylessNewMacro(Self)
-    itkCloneMacro(Self)
+    itkFactorylessNewMacro(Self);
+    itkCloneMacro(Self);
 
+    struct trackingData
+    {
+      mitk::Point3D pos;
+      mitk::Quaternion rot;
+    };
 
     /**
     * \brief Opens the connection to the device and makes it ready to track tools.
     * \return Returns true if there is a connection to the device and the device is ready to track tools, false if not.
     */
     bool StartTracking();
 
     /**
     * \brief Clears all resources. After this method have been called the system isn't ready to track any longer.
     * \return Returns true if the operation was succesful, false if not.
     */
     bool StopTracking();
 
+    bool Connect();
+
+    bool Disconnect();
+
+
+    std::vector<trackingData> GetLastFrame();
+
+    unsigned int GetNumberOfTools();
+
   protected:
     /**
     * \brief standard constructor
     */
     PolhemusInterface();
     /**
     * \brief standard destructor
     */
     ~PolhemusInterface();
 
+    /** Polhemus liberty/patriot tracker object*/
+    CPDIdev m_pdiDev;
+
+    unsigned int m_numberOfTools;
+
+
   };
 }//mitk
 #endif