diff --git a/cost_function.h b/cost_function.h
index cce0ba2..0721c14 100644
--- a/cost_function.h
+++ b/cost_function.h
@@ -21,22 +21,23 @@ struct NegativeLogDataTermGrd
 {
 	using CameraParameters = MultiLayerStixelWorld::CameraParameters;
 
-	NegativeLogDataTermGrd(float dmax, float dmin, float sigmaD, float pOut, float pInv, const CameraParameters& camera,
+	NegativeLogDataTermGrd(float dmax, float dmin, float sigmaD, float pOut, float pInvC, float pInvD, const CameraParameters& camera,
 		const std::vector<float>& groundDisparity, float vhor, float sigmaH, float sigmaA)
 	{
-		init(dmax, dmin, sigmaD, pOut, pInv, camera, groundDisparity, vhor, sigmaH, sigmaA);
+		init(dmax, dmin, sigmaD, pOut, pInvC, pInvD, camera, groundDisparity, vhor, sigmaH, sigmaA);
 	}
 
 	inline float operator()(float d, int v) const
 	{
 		if (d < 0.f)
-			return 0.f;
+			return nLogPInvD_;
 
-		return std::min(nLogPUniform_, nLogPGaussian_[v] + cquad_[v] * (d - fn_[v]) * (d - fn_[v]));
+		const float nLogPData = std::min(nLogPUniform_, nLogPGaussian_[v] + cquad_[v] * (d - fn_[v]) * (d - fn_[v]));
+		return nLogPData + nLogPValD_;
 	}
 
 	// pre-compute constant terms
-	void init(float dmax, float dmin, float sigmaD, float pOut, float pInv, const CameraParameters& camera,
+	void init(float dmax, float dmin, float sigmaD, float pOut, float pInvC, float pInvD, const CameraParameters& camera,
 		const std::vector<float>& groundDisparity, float vhor, float sigmaH, float sigmaA)
 	{
 		// uniform distribution term
@@ -63,9 +64,14 @@ struct NegativeLogDataTermGrd
 			// coefficient of quadratic part
 			cquad_[v] = 1.f / (2.f * sigma * sigma);
 		}
+
+		// probability of invalid and valid disparity
+		pInvD *= pInvC / 3;
+		nLogPInvD_ = -logf(pInvD);
+		nLogPValD_ = -logf(1.f - pInvD);
 	}
 
-	float nLogPUniform_;
+	float nLogPUniform_, nLogPInvD_, nLogPValD_;
 	std::vector<float> nLogPGaussian_, cquad_, fn_;
 };
 
@@ -73,21 +79,22 @@ struct NegativeLogDataTermObj
 {
 	using CameraParameters = MultiLayerStixelWorld::CameraParameters;
 
-	NegativeLogDataTermObj(float dmax, float dmin, float sigma, float pOut, float pInv, const CameraParameters& camera, float deltaz)
+	NegativeLogDataTermObj(float dmax, float dmin, float sigma, float pOut, float pInvC, float pInvD, const CameraParameters& camera, float deltaz)
 	{
-		init(dmax, dmin, sigma, pOut, pInv, camera, deltaz);
+		init(dmax, dmin, sigma, pOut, pInvC, pInvD, camera, deltaz);
 	}
 
 	inline float operator()(float d, int fn) const
 	{
 		if (d < 0.f)
-			return 0.f;
+			return nLogPInvD_;
 
-		return std::min(nLogPUniform_, nLogPGaussian_[fn] + cquad_[fn] * (d - fn) * (d - fn));
+		const float nLogPData = std::min(nLogPUniform_, nLogPGaussian_[fn] + cquad_[fn] * (d - fn) * (d - fn));
+		return nLogPData + nLogPValD_;
 	}
 
 	// pre-compute constant terms
-	void init(float dmax, float dmin, float sigmaD, float pOut, float pInv, const CameraParameters& camera, float deltaz)
+	void init(float dmax, float dmin, float sigmaD, float pOut, float pInvC, float pInvD, const CameraParameters& camera, float deltaz)
 	{
 		// uniform distribution term
 		nLogPUniform_ = logf(dmax - dmin) - logf(pOut);
@@ -107,29 +114,35 @@ struct NegativeLogDataTermObj
 			// coefficient of quadratic part
 			cquad_[fn] = 1.f / (2.f * sigma * sigma);
 		}
+
+		// probability of invalid and valid disparity
+		pInvD *= pInvC / 3;
+		nLogPInvD_ = -logf(pInvD);
+		nLogPValD_ = -logf(1.f - pInvD);
 	}
 
-	float nLogPUniform_;
+	float nLogPUniform_, nLogPInvD_, nLogPValD_;
 	std::vector<float> nLogPGaussian_, cquad_;
 };
 
 struct NegativeLogDataTermSky
 {
-	NegativeLogDataTermSky(float dmax, float dmin, float sigmaD, float pOut, float pInv, float fn = 0.f) : fn_(fn)
+	NegativeLogDataTermSky(float dmax, float dmin, float sigmaD, float pOut, float pInvC, float pInvD, float fn = 0.f) : fn_(fn)
 	{
-		init(dmax, dmin, sigmaD, pOut, pInv, fn);
+		init(dmax, dmin, sigmaD, pOut, pInvC, pInvD, fn);
 	}
 
 	inline float operator()(float d) const
 	{
 		if (d < 0.f)
-			return 0.f;
+			return nLogPInvD_;
 
-		return std::min(nLogPUniform_, nLogPGaussian_ + cquad_ * (d - fn_) * (d - fn_));
+		const float nLogPData = std::min(nLogPUniform_, nLogPGaussian_ + cquad_ * (d - fn_) * (d - fn_));
+		return nLogPData + nLogPValD_;
 	}
 
 	// pre-compute constant terms
-	void init(float dmax, float dmin, float sigmaD, float pOut, float pInv, float fn)
+	void init(float dmax, float dmin, float sigmaD, float pOut, float pInvC, float pInvD, float fn)
 	{
 		// uniform distribution term
 		nLogPUniform_ = logf(dmax - dmin) - logf(pOut);
@@ -140,9 +153,14 @@ struct NegativeLogDataTermSky
 
 		// coefficient of quadratic part
 		cquad_ = 1.f / (2.f * sigmaD * sigmaD);
+
+		// probability of invalid and valid disparity
+		pInvD *= pInvC / 3;
+		nLogPInvD_ = -logf(pInvD);
+		nLogPValD_ = -logf(1.f - pInvD);
 	}
 
-	float nLogPUniform_, cquad_, nLogPGaussian_, fn_;
+	float nLogPUniform_, cquad_, nLogPGaussian_, fn_, nLogPInvD_, nLogPValD_;
 };
 
 //////////////////////////////////////////////////////////////////////////////
diff --git a/multilayer_stixel_world.cpp b/multilayer_stixel_world.cpp
index 2e75e70..d65bd53 100644
--- a/multilayer_stixel_world.cpp
+++ b/multilayer_stixel_world.cpp
@@ -169,10 +169,11 @@ void MultiLayerStixelWorld::compute(const cv::Mat& disparity, std::vector<Stixel
 	const float vhor = h - 1 + line.b / line.a;
 
 	// create data cost function of each segment
-	NegativeLogDataTermGrd dataTermG(param_.dmax, param_.dmin, param_.sigmaG, param_.pOutG, param_.pInvG, camera,
-		groundDisparity, vhor, param_.sigmaH, param_.sigmaA);
-	NegativeLogDataTermObj dataTermO(param_.dmax, param_.dmin, param_.sigmaO, param_.pOutO, param_.pInvO, camera, param_.deltaz);
-	NegativeLogDataTermSky dataTermS(param_.dmax, param_.dmin, param_.sigmaS, param_.pOutS, param_.pInvS);
+	NegativeLogDataTermGrd dataTermG(param_.dmax, param_.dmin, param_.sigmaG, param_.pOutG, param_.pInvG, param_.pInvD,
+		camera, groundDisparity, vhor, param_.sigmaH, param_.sigmaA);
+	NegativeLogDataTermObj dataTermO(param_.dmax, param_.dmin, param_.sigmaO, param_.pOutO, param_.pInvO, param_.pInvD,
+		camera, param_.deltaz);
+	NegativeLogDataTermSky dataTermS(param_.dmax, param_.dmin, param_.sigmaS, param_.pOutS, param_.pInvS, param_.pInvD);
 
 	// create prior cost function of each segment
 	const int G = NegativeLogPriorTerm::G;
diff --git a/multilayer_stixel_world.h b/multilayer_stixel_world.h
index df391a5..0fc4f44 100644
--- a/multilayer_stixel_world.h
+++ b/multilayer_stixel_world.h
@@ -69,6 +69,7 @@ class MultiLayerStixelWorld
 		float pOutS;
 
 		// probability of invalid disparity
+		float pInvD;
 		float pInvG;
 		float pInvO;
 		float pInvS;
@@ -112,6 +113,7 @@ class MultiLayerStixelWorld
 			pOutS = 0.4f;
 
 			// probability of invalid disparity
+			pInvD = 0.25f;
 			pInvG = 0.34f;
 			pInvO = 0.3f;
 			pInvS = 0.36f;