Initialize state estimation signal with initial state provided by command

  - At entity construction, x_est and P depend on no signals
  - when initializing the entity with initial state estimation and variance,
  - signal x_est is set to this initial values and signal dependencies are
    set.

include/sot/core/kalman.hh

@@ -149,11 +149,13 @@ protected:
   void setStateEstimation (const Vector& x0)
   {
     stateEstimation_ = x0;
+    stateUpdateSOUT.recompute (0);
   }
 
   void setStateVariance (const Matrix& P0)
   {
     stateVariance_ = P0;
+    varianceUpdateSOUT.recompute (0);
   }
   // Current state estimation
   // ^

src/tools/kalman.cpp

@@ -45,21 +45,18 @@ namespace dynamicgraph {
 
       ,statePredictedSIN (0, "Kalman("+name+")::input(vector)::x_pred")
       ,observationPredictedSIN (0, "Kalman("+name+")::input(vector)::y_pred")
-      ,varianceUpdateSOUT (boost::bind(&Kalman::computeVarianceUpdate,
-				       this,_1,_2),
-			   noiseTransitionSIN << modelTransitionSIN,
-			   "Kalman("+name+")::output(vector)::P")
-      ,stateUpdateSOUT (boost::bind(&Kalman::computeStateUpdate, this, _1, _2),
-			measureSIN << observationPredictedSIN
-			<< modelTransitionSIN
-			<< modelMeasureSIN << noiseTransitionSIN
-			<< noiseMeasureSIN << statePredictedSIN
-			<< varianceUpdateSOUT,
-			"Kalman("+name+")::output(vector)::x_est"),
+      ,varianceUpdateSOUT ("Kalman("+name+")::output(vector)::P")
+      ,stateUpdateSOUT ("Kalman("+name+")::output(vector)::x_est"),
 	stateEstimation_ (),
 	stateVariance_ ()
     {
       sotDEBUGIN(15);
+      varianceUpdateSOUT.setFunction
+	(boost::bind(&Kalman::computeVarianceUpdate,
+		     this,_1,_2));
+      stateUpdateSOUT.setFunction (boost::bind(&Kalman::computeStateUpdate,
+					       this, _1, _2));
+
       signalRegistration( measureSIN << observationPredictedSIN
 			  << modelTransitionSIN
 			  << modelMeasureSIN << noiseTransitionSIN
@@ -92,39 +89,47 @@ namespace dynamicgraph {
     computeVarianceUpdate (Matrix& Pk_k,const int& time)
     {
       sotDEBUGIN(15);
-
-      const Matrix &Q = noiseTransitionSIN( time );
-      const Matrix& R = noiseMeasureSIN (time);
-      const Matrix &F = modelTransitionSIN( time );
-      const Matrix& H = modelMeasureSIN (time);
-      const Matrix &Pk_1_k_1 = stateVariance_;
-
-      sotDEBUG(15) << "Q=" << Q << std::endl;
-      sotDEBUG(15) << "R=" << R << std::endl;
-      sotDEBUG(15) << "F=" << F << std::endl;
-      sotDEBUG(15) << "H=" << H << std::endl;
-      sotDEBUG(15) << "Pk_1_k_1=" << Pk_1_k_1 << std::endl;
-
-      F.multiply(Pk_1_k_1,FP_);
-      FP_.multiply (F.transpose(), Pk_k_1_);
-      Pk_k_1_ += Q;
-
-      sotDEBUG(15) << "F " <<std::endl << F << std::endl;
-      sotDEBUG(15) << "P_{k-1|k-1} " <<std::endl<< Pk_1_k_1 << std::endl;
-      sotDEBUG(15) << "F^T " <<std::endl<< F.transpose() << std::endl;
-      sotDEBUG(15) << "P_{k|k-1} " << std::endl << Pk_k_1_ << std::endl;
-
-      S_ = H * Pk_k_1_ * H.transpose () + R;
-      K_ = Pk_k_1_ * H.transpose () * S_.inverse ();
-      Pk_k = Pk_k_1_ - K_ * H * Pk_k_1_;
-
-      sotDEBUG (15) << "S_{k} " << std::endl << S_ << std::endl;
-      sotDEBUG (15) << "K_{k} " << std::endl << K_ << std::endl;
-      sotDEBUG (15) << "P_{k|k} " << std::endl << Pk_k << std::endl;
-
-      sotDEBUGOUT(15);
-
-      stateVariance_ = Pk_k;
+      if (time == 0) {
+	// First time return variance initial state
+	Pk_k = stateVariance_;
+	// Set dependency to input signals for latter computations
+	varianceUpdateSOUT.addDependency (noiseTransitionSIN);
+	varianceUpdateSOUT.addDependency (modelTransitionSIN);
+      } else {
+
+	const Matrix &Q = noiseTransitionSIN( time );
+	const Matrix& R = noiseMeasureSIN (time);
+	const Matrix &F = modelTransitionSIN( time );
+	const Matrix& H = modelMeasureSIN (time);
+	const Matrix &Pk_1_k_1 = stateVariance_;
+
+	sotDEBUG(15) << "Q=" << Q << std::endl;
+	sotDEBUG(15) << "R=" << R << std::endl;
+	sotDEBUG(15) << "F=" << F << std::endl;
+	sotDEBUG(15) << "H=" << H << std::endl;
+	sotDEBUG(15) << "Pk_1_k_1=" << Pk_1_k_1 << std::endl;
+
+	F.multiply(Pk_1_k_1,FP_);
+	FP_.multiply (F.transpose(), Pk_k_1_);
+	Pk_k_1_ += Q;
+
+	sotDEBUG(15) << "F " <<std::endl << F << std::endl;
+	sotDEBUG(15) << "P_{k-1|k-1} " <<std::endl<< Pk_1_k_1 << std::endl;
+	sotDEBUG(15) << "F^T " <<std::endl<< F.transpose() << std::endl;
+	sotDEBUG(15) << "P_{k|k-1} " << std::endl << Pk_k_1_ << std::endl;
+
+	S_ = H * Pk_k_1_ * H.transpose () + R;
+	K_ = Pk_k_1_ * H.transpose () * S_.inverse ();
+	Pk_k = Pk_k_1_ - K_ * H * Pk_k_1_;
+
+	sotDEBUG (15) << "S_{k} " << std::endl << S_ << std::endl;
+	sotDEBUG (15) << "K_{k} " << std::endl << K_ << std::endl;
+	sotDEBUG (15) << "P_{k|k} " << std::endl << Pk_k << std::endl;
+
+	sotDEBUGOUT(15);
+
+	stateVariance_ = Pk_k;
+      }
       return Pk_k;
     }
 
@@ -151,28 +156,39 @@ namespace dynamicgraph {
     computeStateUpdate (Vector& x_est,const int& time )
     {
       sotDEBUGIN(15);
-
-      varianceUpdateSOUT.recompute (time);
-      const Vector & x_pred = statePredictedSIN (time);
-      const Vector & y_pred = observationPredictedSIN (time);
-      const Vector & y = measureSIN (time);
-
-      sotDEBUG(25) << "K_{k} = "<<std::endl<< K_ << std::endl;
-      sotDEBUG(25) << "y = " << y << std::endl;
-      sotDEBUG(25) << "h (\\hat{x}_{k|k-1}) = " << y_pred << std::endl;
-      sotDEBUG(25) << "y = " << y << std::endl;
-    
-      // Innovation: z_ = y - Hx
-      z_ = y - y_pred;
-      //x_est = x_pred + (K*(y-(H*x_pred)));
-      x_est = x_pred + K_ * z_;
-
-      sotDEBUG(25) << "z_{k} = " << z_ << std::endl;
-      sotDEBUG(25) << "x_{k|k} = " << x_est << std::endl;
-    
+      if (time == 0) {
+	// First time return variance initial state
+	x_est = stateEstimation_;
+	// Set dependency to input signals for latter computations
+	stateUpdateSOUT.addDependency (measureSIN);
+	stateUpdateSOUT.addDependency (observationPredictedSIN);
+	stateUpdateSOUT.addDependency (modelMeasureSIN);
+	stateUpdateSOUT.addDependency (noiseTransitionSIN);
+	stateUpdateSOUT.addDependency (noiseMeasureSIN);
+	stateUpdateSOUT.addDependency (statePredictedSIN);
+	stateUpdateSOUT.addDependency (varianceUpdateSOUT);
+      } else {
+	varianceUpdateSOUT.recompute (time);
+	const Vector & x_pred = statePredictedSIN (time);
+	const Vector & y_pred = observationPredictedSIN (time);
+	const Vector & y = measureSIN (time);
+
+	sotDEBUG(25) << "K_{k} = "<<std::endl<< K_ << std::endl;
+	sotDEBUG(25) << "y = " << y << std::endl;
+	sotDEBUG(25) << "h (\\hat{x}_{k|k-1}) = " << y_pred << std::endl;
+	sotDEBUG(25) << "y = " << y << std::endl;
+
+	// Innovation: z_ = y - Hx
+	z_ = y - y_pred;
+	//x_est = x_pred + (K*(y-(H*x_pred)));
+	x_est = x_pred + K_ * z_;
+
+	sotDEBUG(25) << "z_{k} = " << z_ << std::endl;
+	sotDEBUG(25) << "x_{k|k} = " << x_est << std::endl;
+
+	stateEstimation_ = x_est;
+      }
       sotDEBUGOUT (15);
-
-      stateEstimation_ = x_est;
       return x_est;
     }