diff --git a/Tracking/TrkEvent/TrkMultiComponentStateOnSurface/TrkMultiComponentStateOnSurface/MultiComponentState.h b/Tracking/TrkEvent/TrkMultiComponentStateOnSurface/TrkMultiComponentStateOnSurface/MultiComponentState.h
index a8f0bdfec77778b535d9280e2b46ec952f7b5d70..3d3c3c466caf3ef872a90dbd809960fb449d3bdb 100755
--- a/Tracking/TrkEvent/TrkMultiComponentStateOnSurface/TrkMultiComponentStateOnSurface/MultiComponentState.h
+++ b/Tracking/TrkEvent/TrkMultiComponentStateOnSurface/TrkMultiComponentStateOnSurface/MultiComponentState.h
@@ -40,26 +40,26 @@ class MultiComponentState : public std::list<ComponentParameters>{
   virtual ~MultiComponentState();
 
   /** Clone method */
-  virtual const MultiComponentState* clone() const;
+  virtual MultiComponentState* clone() const;
 
   /** Clone with rescaled weight scaling factor */
-  virtual const MultiComponentState* cloneWithWeightScaling( double ) const;
+  virtual MultiComponentState* cloneWithWeightScaling( double ) const;
 
   /** Clone with covariance matricies scaled by a factor */
-  virtual const MultiComponentState* cloneWithScaledError( double ) const;
+  virtual MultiComponentState* cloneWithScaledError( double ) const;
 
   /** Clone with covariance matrix componants scaled by individual factors
       This will only work if there are 5 track parameters in each componant
   */
-  virtual const MultiComponentState* cloneWithScaledError( double, double,
-                                                           double, double, 
-                                                           double ) const;
+  virtual MultiComponentState* cloneWithScaledError( double, double,
+                                                     double, double, 
+                                                     double ) const;
 
   /** Check to see if all components in the state have measured track parameters */
   virtual bool isMeasured() const;
 
   /** Clone state performing renormalisation of total state weighting to one */
-  virtual const MultiComponentState* clonedRenormalisedState() const;
+  virtual MultiComponentState* clonedRenormalisedState() const;
 
   /** Dump methods */
   virtual MsgStream&    dump( MsgStream& ) const;
diff --git a/Tracking/TrkEvent/TrkMultiComponentStateOnSurface/src/MultiComponentState.cxx b/Tracking/TrkEvent/TrkMultiComponentStateOnSurface/src/MultiComponentState.cxx
index 714560052b542897c9dda4d8e977ef2b5136df0d..af9e4cb025ea6f3ef20f27ff0c0fe7c81e350eb4 100755
--- a/Tracking/TrkEvent/TrkMultiComponentStateOnSurface/src/MultiComponentState.cxx
+++ b/Tracking/TrkEvent/TrkMultiComponentStateOnSurface/src/MultiComponentState.cxx
@@ -38,7 +38,7 @@ Trk::MultiComponentState::~MultiComponentState()
   this->clear(); 
 }
 
-const Trk::MultiComponentState* Trk::MultiComponentState::clone() const
+Trk::MultiComponentState* Trk::MultiComponentState::clone() const
 {
   Trk::MultiComponentState* clonedState = new Trk::MultiComponentState();
   Trk::MultiComponentState::const_iterator component = this->begin();
@@ -51,7 +51,7 @@ const Trk::MultiComponentState* Trk::MultiComponentState::clone() const
 }
 
 
-const Trk::MultiComponentState* Trk::MultiComponentState::cloneWithWeightScaling( double scalingFactor ) const
+Trk::MultiComponentState* Trk::MultiComponentState::cloneWithWeightScaling( double scalingFactor ) const
 {
   Trk::MultiComponentState* clonedState = new Trk::MultiComponentState();
   Trk::MultiComponentState::const_iterator component = this->begin();
@@ -65,8 +65,8 @@ const Trk::MultiComponentState* Trk::MultiComponentState::cloneWithWeightScaling
 }
 
 
-const Trk::MultiComponentState* Trk::MultiComponentState::cloneWithScaledError( double errorScaleLocX,double errorScaleLocY, double errorScalePhi,
-                                                                                double errorScaleTheta,double errorScaleQoverP ) const
+Trk::MultiComponentState* Trk::MultiComponentState::cloneWithScaledError( double errorScaleLocX,double errorScaleLocY, double errorScalePhi,
+                                                                          double errorScaleTheta,double errorScaleQoverP ) const
 {
   Trk::MultiComponentState* stateWithScaledErrors = new Trk::MultiComponentState();
   Trk::MultiComponentState::const_iterator component = this->begin();
@@ -131,7 +131,7 @@ const Trk::MultiComponentState* Trk::MultiComponentState::cloneWithScaledError(
 
 
 
-const Trk::MultiComponentState* Trk::MultiComponentState::cloneWithScaledError( double errorScale ) const
+Trk::MultiComponentState* Trk::MultiComponentState::cloneWithScaledError( double errorScale ) const
 {
 
   Trk::MultiComponentState* stateWithScaledErrors = new Trk::MultiComponentState();
@@ -214,7 +214,7 @@ bool Trk::MultiComponentState::isMeasured() const
 
 }
 
-const Trk::MultiComponentState* Trk::MultiComponentState::clonedRenormalisedState() const
+Trk::MultiComponentState* Trk::MultiComponentState::clonedRenormalisedState() const
 {
 
   Trk::MultiComponentState::const_iterator component = this->begin();
diff --git a/Tracking/TrkFitter/TrkGaussianSumFilter/TrkGaussianSumFilter/IPosteriorWeightsCalculator.h b/Tracking/TrkFitter/TrkGaussianSumFilter/TrkGaussianSumFilter/IPosteriorWeightsCalculator.h
index 9818f2e19fad13a7fb772c1c1fe0c18b56fd49d3..b01eb1678fcb42515b9aadb9881a3f78e7dc7168 100755
--- a/Tracking/TrkFitter/TrkGaussianSumFilter/TrkGaussianSumFilter/IPosteriorWeightsCalculator.h
+++ b/Tracking/TrkFitter/TrkGaussianSumFilter/TrkGaussianSumFilter/IPosteriorWeightsCalculator.h
@@ -34,7 +34,7 @@ class IPosteriorWeightsCalculator : virtual public IAlgTool {
   static const InterfaceID& interfaceID () { return IID_PosteriorWeightsCalculator; };
 
   /** Method to compute the state weights after measurement update */
-  virtual const MultiComponentState* weights( const MultiComponentState&, const MeasurementBase& ) = 0;
+  virtual const MultiComponentState* weights( const MultiComponentState&, const MeasurementBase& ) const = 0;
  
 };
 
diff --git a/Tracking/TrkFitter/TrkGaussianSumFilter/TrkGaussianSumFilter/PosteriorWeightsCalculator.h b/Tracking/TrkFitter/TrkGaussianSumFilter/TrkGaussianSumFilter/PosteriorWeightsCalculator.h
index fd2e9ebd70757275fbdd83c68381eca4cb748329..432211e1df384aa34b36d27e1e7e807633dc2b32 100755
--- a/Tracking/TrkFitter/TrkGaussianSumFilter/TrkGaussianSumFilter/PosteriorWeightsCalculator.h
+++ b/Tracking/TrkFitter/TrkGaussianSumFilter/TrkGaussianSumFilter/PosteriorWeightsCalculator.h
@@ -43,7 +43,7 @@ class PosteriorWeightsCalculator : public AthAlgTool, virtual public IPosteriorW
   StatusCode finalize();
 
   /** Method to compute the state weights after measurement update */
-  virtual const MultiComponentState* weights( const MultiComponentState&, const MeasurementBase& );
+  virtual const MultiComponentState* weights( const MultiComponentState&, const MeasurementBase& ) const;
  
 };
 
diff --git a/Tracking/TrkFitter/TrkGaussianSumFilter/src/CloseComponentsMultiStateMerger.cxx b/Tracking/TrkFitter/TrkGaussianSumFilter/src/CloseComponentsMultiStateMerger.cxx
index cdcc903ccf986cd5eab1d0444ccade99d58d1b16..147e8674bfaec0a937fa682f322925b242c769dd 100644
--- a/Tracking/TrkFitter/TrkGaussianSumFilter/src/CloseComponentsMultiStateMerger.cxx
+++ b/Tracking/TrkFitter/TrkGaussianSumFilter/src/CloseComponentsMultiStateMerger.cxx
@@ -151,7 +151,7 @@ const Trk::MultiComponentState* Trk::CloseComponentsMultiStateMerger::merge(cons
 
   // Clone unmerged state as STL methods deletes components
 
-  Trk::MultiComponentState* clonedUnmergedState = const_cast<Trk::MultiComponentState*>( unmergedState.clone() );
+  Trk::MultiComponentState* clonedUnmergedState = unmergedState.clone();
 
   component = clonedUnmergedState->begin();
 
diff --git a/Tracking/TrkFitter/TrkGaussianSumFilter/src/GsfMeasurementUpdator.cxx b/Tracking/TrkFitter/TrkGaussianSumFilter/src/GsfMeasurementUpdator.cxx
index 005d4c24940efb98a312ff52cbfcf9a2ed05eed4..06ca7025dc515f44fb98f7d2b3d1a55589cc70a1 100644
--- a/Tracking/TrkFitter/TrkGaussianSumFilter/src/GsfMeasurementUpdator.cxx
+++ b/Tracking/TrkFitter/TrkGaussianSumFilter/src/GsfMeasurementUpdator.cxx
@@ -231,11 +231,8 @@ Trk::GsfMeasurementUpdator::calculateFilterStep( const Trk::MultiComponentState&
 
     Trk::FitQualityOnSurface* fitQuality = 0;
 
-    // Track update alternates between update and getUnbiasedTrackParams
-    Trk::IUpdator* updatorPointer = &(*m_updator);
-    updatedTrackParameters = (updatorPointer->*updator) ( *(*component).first, measurement.localParameters(), measurement.localCovariance(), fitQuality );
-
-    //updatedTrackParameters = m_updator->addToState( *(*component).first, measurement.localParameters(), measurement.localCovariance(), fitQuality );
+    // Track updates using a pointer to the member function
+    updatedTrackParameters = ((&(*m_updator))->*updator)( *(*component).first, measurement.localParameters(), measurement.localCovariance(), fitQuality );
 
     if ( !updatedTrackParameters ) {
       ATH_MSG_DEBUG( "Update of state with Measurement has failed 1... Exiting!");
diff --git a/Tracking/TrkFitter/TrkGaussianSumFilter/src/LargestWeightsMultiStateMerger.cxx b/Tracking/TrkFitter/TrkGaussianSumFilter/src/LargestWeightsMultiStateMerger.cxx
index 6697551091149795bf1cd4e7f6b09cd4ce446efe..05b68949c15998b0a7f1514e4834d8289b924416 100644
--- a/Tracking/TrkFitter/TrkGaussianSumFilter/src/LargestWeightsMultiStateMerger.cxx
+++ b/Tracking/TrkFitter/TrkGaussianSumFilter/src/LargestWeightsMultiStateMerger.cxx
@@ -94,7 +94,7 @@ const Trk::MultiComponentState* Trk::LargestWeightsMultiStateMerger::merge(const
     return multiComponentState.clone();
   }
 
-  Trk::MultiComponentState* unmergedState = const_cast<Trk::MultiComponentState*>( multiComponentState.clone() );
+  Trk::MultiComponentState* unmergedState = multiComponentState.clone();
 
   std::vector<double> weights;
   Trk::MultiComponentState* componentsForCollapse = new Trk::MultiComponentState;
diff --git a/Tracking/TrkFitter/TrkGaussianSumFilter/src/MultiComponentStateAssembler.cxx b/Tracking/TrkFitter/TrkGaussianSumFilter/src/MultiComponentStateAssembler.cxx
index c5070058c1ea6a56604443981a7ba995be13057a..08766b466a7228010964df533d1d42288a1932cd 100755
--- a/Tracking/TrkFitter/TrkGaussianSumFilter/src/MultiComponentStateAssembler.cxx
+++ b/Tracking/TrkFitter/TrkGaussianSumFilter/src/MultiComponentStateAssembler.cxx
@@ -95,7 +95,7 @@ bool Trk::MultiComponentStateAssembler::addMultiState (Cache& cache, const Multi
     return false;
   }
 
-  Trk::MultiComponentState* clonedMultiComponentState = const_cast<Trk::MultiComponentState*>( multiComponentState.clone() );
+  Trk::MultiComponentState* clonedMultiComponentState = multiComponentState.clone();
 
   this->addComponentsList(cache,clonedMultiComponentState);
   clonedMultiComponentState->clear();
diff --git a/Tracking/TrkFitter/TrkGaussianSumFilter/src/PosteriorWeightsCalculator.cxx b/Tracking/TrkFitter/TrkGaussianSumFilter/src/PosteriorWeightsCalculator.cxx
index 17a27a16a484979f59834cfeeed95641660ec1ca..2adf3c49f9a1bb1072b8d502b43accb2aa5d4d2e 100755
--- a/Tracking/TrkFitter/TrkGaussianSumFilter/src/PosteriorWeightsCalculator.cxx
+++ b/Tracking/TrkFitter/TrkGaussianSumFilter/src/PosteriorWeightsCalculator.cxx
@@ -32,7 +32,7 @@ Trk::PosteriorWeightsCalculator::PosteriorWeightsCalculator(const std::string& t
 StatusCode Trk::PosteriorWeightsCalculator::initialize() 
 {
 
-  msg(MSG::INFO) << "Initialisation of " << name() << " was successful" << endmsg; 
+  ATH_MSG_INFO( "Initialisation of " << name() << " was successful" ); 
 
   return StatusCode::SUCCESS;
 
@@ -41,23 +41,23 @@ StatusCode Trk::PosteriorWeightsCalculator::initialize()
 StatusCode Trk::PosteriorWeightsCalculator::finalize() 
 {
 
-  msg(MSG::INFO) << "Finalisation of " << name() << " was successful" << endmsg;
+  ATH_MSG_INFO( "Finalisation of " << name() << " was successful" );
 
   return StatusCode::SUCCESS;
 
 }
 
 const Trk::MultiComponentState*
-Trk::PosteriorWeightsCalculator::weights ( const MultiComponentState& predictedState, const MeasurementBase& measurement ){
+Trk::PosteriorWeightsCalculator::weights ( const MultiComponentState& predictedState, const MeasurementBase& measurement ) const{
   
-  if (msgLvl(MSG::VERBOSE)) msg() << "Calculating Posterior Weights" << endmsg;
+  ATH_MSG_VERBOSE( "Calculating Posterior Weights" );
   
   if ( predictedState.empty() ) {
-    msg(MSG::WARNING) << "Predicted state is empty... Exiting!" << endmsg;
+    ATH_MSG_WARNING("Predicted state is empty... Exiting!" );
     return 0;
   }
   
-  if (msgLvl(MSG::VERBOSE)) msg() << "State for update is valid!" << endmsg;
+  ATH_MSG_VERBOSE( "State for update is valid!" );
     
   Trk::MultiComponentState* returnMultiComponentState = new Trk::MultiComponentState();
   
@@ -75,18 +75,18 @@ Trk::PosteriorWeightsCalculator::weights ( const MultiComponentState& predictedS
     const Trk::TrackParameters* componentTrackParameters = (*component).first;
 
     if ( !componentTrackParameters ) {
-      if (msgLvl(MSG::DEBUG)) msg() << "Component in the state prepared for update is invalid... Ignoring!" << endmsg;
+      ATH_MSG_DEBUG( "Component in the state prepared for update is invalid... Ignoring!" );
       continue;
     }
     
     const AmgSymMatrix(5)* predictedCov = componentTrackParameters->covariance();
     
     if (!predictedCov){
-      msg(MSG::WARNING) << "No measurement associated with track parameters... Ignoring!" << endmsg;
+      ATH_MSG_WARNING( "No measurement associated with track parameters... Ignoring!" );
       continue;
     }
 
-    if (msgLvl(MSG::VERBOSE)) msg() << "Component for update is valid!" << endmsg;
+    ATH_MSG_VERBOSE ( "Component for update is valid!" );
     
     // Extract the LocalParameters from the MeasurementBase
     const Trk::LocalParameters& measurementLocalParameters = measurement.localParameters();
@@ -110,7 +110,7 @@ Trk::PosteriorWeightsCalculator::weights ( const MultiComponentState& predictedS
     double determinantR = R.determinant();
 
     if (determinantR==0){
-      msg(MSG::WARNING) << "Determinant is 0, cannot invert matrix... Ignoring component" << endmsg;
+      ATH_MSG_WARNING( "Determinant is 0, cannot invert matrix... Ignoring component" );
       continue;
     }
     // Compute Chi2
@@ -118,7 +118,7 @@ Trk::PosteriorWeightsCalculator::weights ( const MultiComponentState& predictedS
 
     double chi2 = (1./size)*((r.transpose() * R.inverse() * r)(0,0));
  
-    if (msgLvl(MSG::VERBOSE)) msg() << "determinant R / chiSquared: " << determinantR << '\t' << chi2 << endmsg;
+    ATH_MSG_VERBOSE( "determinant R / chiSquared: " << determinantR << '\t' << chi2 );
 
     componentDeterminantR.push_back(determinantR);
     componentChi2.push_back(chi2);
@@ -129,7 +129,7 @@ Trk::PosteriorWeightsCalculator::weights ( const MultiComponentState& predictedS
   } // end loop over components
   
   if ( componentDeterminantR.size() != predictedState.size() || componentChi2.size() != predictedState.size() ){
-    msg(MSG::WARNING) << "Inconsistent number of components in chi2 and detR vectors... Exiting!" << endmsg;
+    ATH_MSG_WARNING("Inconsistent number of components in chi2 and detR vectors... Exiting!" );
     return 0;
   }
   
@@ -164,7 +164,7 @@ Trk::PosteriorWeightsCalculator::weights ( const MultiComponentState& predictedS
   }
   
   if ( returnMultiComponentState->size() != predictedState.size() ){
-    msg(MSG::WARNING) << "Inconsistent number of components between initial and final states... Exiting!" << endmsg;
+    ATH_MSG_WARNING( "Inconsistent number of components between initial and final states... Exiting!" );
     return 0;
   }
   
@@ -176,7 +176,7 @@ Trk::PosteriorWeightsCalculator::weights ( const MultiComponentState& predictedS
     if (sumWeights > 0. ){
       (*returnComponent).second /= sumWeights;
     } else {
-      (*returnComponent).second  = component->second;
+      (*returnComponent).second  = component->second;     
     }
   }  
 
diff --git a/Tracking/TrkFitter/TrkGaussianSumFilter/src/QuickCloseComponentsMultiStateMerger.cxx b/Tracking/TrkFitter/TrkGaussianSumFilter/src/QuickCloseComponentsMultiStateMerger.cxx
index c7373c2c2b571410223329f7ad7ab31e96040e54..665fe7ba57ca062d45eb459e09e246be483f008c 100644
--- a/Tracking/TrkFitter/TrkGaussianSumFilter/src/QuickCloseComponentsMultiStateMerger.cxx
+++ b/Tracking/TrkFitter/TrkGaussianSumFilter/src/QuickCloseComponentsMultiStateMerger.cxx
@@ -192,7 +192,7 @@ const Trk::MultiComponentState* Trk::QuickCloseComponentsMultiStateMerger::merge
   MultiComponentStateMap unmergedComponentsMap(comparator);
   
   // Clone unmerged state as STL methods deletes components
-  Trk::MultiComponentState* clonedUnmergedState = const_cast<Trk::MultiComponentState*>( unmergedState.clone() );
+  Trk::MultiComponentState* clonedUnmergedState = unmergedState.clone();
 
   component = clonedUnmergedState->begin();