From: Lukáš Jiřiště <jiriste@icpf.cas.cz>
Date: Mon, 16 Dec 2024 09:32:12 +0000 (+0100)
Subject: Improve "fill" pressure estimate
X-Git-Url: https://git.ljiriste.work/?a=commitdiff_plain;h=802189fc4988cf0d89d7ec8f2637198c1af50258;p=analyse_servo.git

Improve "fill" pressure estimate
---

diff --git a/get_important_pressure.m b/get_important_pressure.m
index 2093515..a75a82a 100644
--- a/get_important_pressure.m
+++ b/get_important_pressure.m
@@ -13,7 +13,7 @@ function [p_eq, p_after_fill] = get_important_pressure(t, p,  graph = 1, thresho
 		p_single = p(open_inds(i - 1):(fill_inds(i) - 1));
 		p_eq(i - 1)  = get_equilibrium_pressure(t_single, p_single);
 		if (i > 2)
-			p_after_fill(i - 1) = get_equilibrium_pressure(t(fill_inds(i - 1):open_inds(i - 1) - 1), p(fill_inds(i - 1):open_inds(i - 1) - 1));
+			p_after_fill(i - 1) = get_fill_pressure(p(fill_inds(i - 1):open_inds(i - 1) - 1));
 		end
 		if (graph)
 			if (i < length(fill_inds))
@@ -33,3 +33,19 @@ function p_eq = get_equilibrium_pressure(t, p)
 	weights = ((t - t(1)) / (t(end) - t(1))) .^ 10;
 	p_eq = weighted_mean(p, weights);
 end
+
+function p_fill = get_fill_pressure(p)
+	p_fill = mean(p);
+	while (numel(p) > 2)
+		p_mean = mean(p);
+		res_sq = (p - p_mean) .^ 2;
+		p_var = mean(res_sq);
+		remove_inds = res_sq > 3 * p_var;
+		if (all(~remove_inds))
+			break ;
+		end
+		[~, remove_inds] = max(res_sq);
+		p(remove_inds) = [];
+	end
+	p_fill = p_mean;
+end