INSTINCT Code Coverage Report

Line	Branch	Exec	Source
1			// This file is part of INSTINCT, the INS Toolkit for Integrated
2			// Navigation Concepts and Training by the Institute of Navigation of
3			// the University of Stuttgart, Germany.
4			//
5			// This Source Code Form is subject to the terms of the Mozilla Public
6			// License, v. 2.0. If a copy of the MPL was not distributed with this
7			// file, You can obtain one at https://mozilla.org/MPL/2.0/.
8
9			#include "ARMA.hpp"
10
11			#include "util/Logger.hpp"
12
13			#include "internal/NodeManager.hpp"
14			namespace nm = NAV::NodeManager;
15			#include "internal/FlowManager.hpp"
16			#include "util/Eigen.hpp"
17			#include <iterator>
18			#include <boost/math/distributions/students_t.hpp>
19
20		✗	NAV::experimental::ARMA::ARMA()
21		✗	: Node(typeStatic())
22			{
23			LOG_TRACE("{}: called", name);
24
25		✗	_hasConfig = true;
26
27		✗	nm::CreateInputPin(this, "ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() }, &ARMA::receiveImuObs);
28
29		✗	nm::CreateOutputPin(this, "ImuObs", Pin::Type::Flow, { NAV::ImuObs::type() });
30		✗	}
31
32		✗	NAV::experimental::ARMA::~ARMA()
33			{
34			LOG_TRACE("{}: called", nameId());
35		✗	}
36
37		✗	std::string NAV::experimental::ARMA::typeStatic()
38			{
39		✗	return "ARMA";
40			}
41
42		✗	std::string NAV::experimental::ARMA::type() const
43			{
44		✗	return typeStatic();
45			}
46
47		✗	std::string NAV::experimental::ARMA::category()
48			{
49		✗	return "Experimental/DataProcessor";
50			}
51
52		✗	void NAV::experimental::ARMA::guiConfig()
53			{
54			// GUI ARMA node input
55		✗	ImGui::InputInt("Deque size", &_deque_size); // int input of modelling size
56		✗	ImGui::InputInt("p", &_p); // int input of initial AR-order
57		✗	ImGui::InputInt("q", &_q); // int input of initial MA-order
58			static ImGuiTableFlags flags = ImGuiTableFlags_Borders | ImGuiTableFlags_RowBg;
59		✗	if (ImGui::BeginTable("##table1", 3, flags)) // display ARMA parameters (phi and theta) in table
60			{
61		✗	ImGui::TableSetupColumn("ARMA Parameter");
62		✗	ImGui::TableSetupColumn("estimate");
63		✗	ImGui::TableSetupColumn("p");
64		✗	ImGui::TableHeadersRow();
65
66		✗	for (int table_row = 0; table_row < _x.size(); table_row++)
67			{
68		✗	ImGui::TableNextRow();
69		✗	ImGui::TableNextColumn();
70		✗	if (table_row < _p) // columns for AR-parameters (phi)
71			{
72		✗	ImGui::Text("phi %d", table_row + 1);
73			}
74			else // columns for MA-parameters (theta)
75			{
76		✗	ImGui::Text("theta %d", table_row - _p + 1);
77			}
78		✗	ImGui::TableNextColumn();
79		✗	ImGui::Text("%f", _x(table_row)); // display parameter
80		✗	ImGui::TableNextColumn();
81		✗	ImGui::Text("%f", _emp_sig(table_row)); // display p-Value of zero slope test
82			}
83		✗	ImGui::EndTable();
84			}
85		✗	}
86
87		✗	[[nodiscard]] json NAV::experimental::ARMA::save() const
88			{
89			LOG_TRACE("{}: called", nameId());
90
91		✗	json j;
92
93		✗	j["deque_size"] = _deque_size;
94		✗	j["p"] = _p;
95		✗	j["q"] = _q;
96
97		✗	return j;
98		✗	}
99
100		✗	void NAV::experimental::ARMA::restore(json const& j)
101			{
102			LOG_TRACE("{}: called", nameId());
103
104		✗	if (j.contains("deque_size"))
105			{
106		✗	j.at("deque_size").get_to(_deque_size);
107			}
108		✗	if (j.contains("p"))
109			{
110		✗	j.at("p").get_to(_p);
111			}
112		✗	if (j.contains("q"))
113			{
114		✗	j.at("q").get_to(_q);
115			}
116		✗	}
117
118		✗	bool NAV::experimental::ARMA::initialize()
119			{
120			LOG_TRACE("{}: called", nameId());
121
122		✗	_buffer.clear(); // clear deque
123
124			// declaration
125		✗	_p_mem = _p; // reset p, q to initial for next observation
126		✗	_q_mem = _q;
127		✗	_y = Eigen::MatrixXd::Zero(_deque_size, _num_obs); // measurement data
128		✗	_y_rbm = Eigen::VectorXd::Zero(_deque_size); // y (reduced by mean)
129		✗	_x = Eigen::VectorXd::Zero(_p + _q); // ARMA slope parameters
130		✗	_emp_sig = Eigen::VectorXd::Zero(_p + _q); // empirical significance (p-Value) of parameters
131		✗	_y_hat = Eigen::VectorXd::Zero(_deque_size); // ARMA estimates for y_rbm
132
133		✗	_m = static_cast<int>(std::max(_p, _q)); // value of superior order (p or q)
134		✗	_y_mean = 0.0;
135		✗	_y_hat_t = Eigen::VectorXd::Zero(_num_obs); // output container
136
137		✗	return true;
138			}
139
140		✗	void NAV::experimental::ARMA::deinitialize()
141			{
142			LOG_TRACE("{}: called", nameId());
143		✗	}
144
145		✗	void NAV::experimental::ARMA::acf_function(Eigen::VectorXd& y, int p, Eigen::VectorXd& acf)
146			{
147		✗	int acf_size = static_cast<int>(y.size()); // size of y
148			/* Calculation of initial ê through Yule-Walker:
149			model approximation through AR(m)-process with m > max(p,q)
150			therefore limited amount of ACF values required to calculate PACF */
151		✗	int p_approx = p + 3; // in this case AR(p + 3)
152
153		✗	double cov = 0.0; // covariance of measured values
154		✗	double var = 0.0; // variance of measured values
155		✗	double mean = y.mean();
156
157		✗	for (int i = 0; i < acf_size; i++)
158			{
159		✗	var += (y(i) - mean) * (y(i) - mean); // sum of variance
160			}
161			// Calculation limited to required ACF values for Yule-Walker
162		✗	for (int tau = 0; tau < p_approx + 1; tau++)
163			{ // acf: correlation to delta_tau
164
165		✗	for (int i = 0; i < acf_size - tau; i++)
166			{
167		✗	cov += (y(i) - mean) * (y(i + tau) - mean); // sum of covariance
168			}
169		✗	acf(tau) = cov / var;
170		✗	cov = 0; // reset 'cov' for next sum
171			}
172		✗	}
173
174		✗	void NAV::experimental::ARMA::pacf_function(Eigen::VectorXd& y, Eigen::VectorXd& acf, int p, Eigen::VectorXd& pacf, Eigen::VectorXd& e_hat_initial)
175			{
176		✗	int pacf_size = static_cast<int>(y.size());
177			/* Calculation of initial ê through Yule-Walker:
178			AR(m) parameters (phi_1...m) are equal to phi_m_1...m from PACF */
179		✗	int p_approx = p + 3; // AR order > max(p,q) to approximate ARMA
180
181		✗	Eigen::VectorXd phi_tau = Eigen::VectorXd::Zero(p_approx); // phi_tau_1...tau
182		✗	Eigen::VectorXd phi_tau_i = Eigen::VectorXd::Zero(p_approx); // phi_tau-1_1...tau-1 (from previous iteration)
183		✗	Eigen::VectorXd phi_initial = Eigen::VectorXd::Zero(p_approx); // Yule-Walker ARMA-Parameters
184
185		✗	double sum1 = 0.0;
186		✗	double sum2 = 0.0;
187		✗	double sum3 = 0.0;
188
189			// PACF while E(eta_t) = 0
190			// first iteration step
191		✗	pacf(0) = acf(1); // initial value phi_1_1 = p(1)
192			#if defined(__GNUC__) || defined(__clang__)
193			#pragma GCC diagnostic push
194			#pragma GCC diagnostic ignored "-Wnull-dereference"
195			#endif
196		✗	phi_tau_i(0) = pacf(0);
197			#if defined(__GNUC__) || defined(__clang__)
198			#pragma GCC diagnostic pop
199			#endif
200		✗	sum1 = acf(1) * acf(1);
201		✗	sum2 = acf(1) * acf(1);
202
203		✗	e_hat_initial(0) = 0; // first entry
204
205		✗	for (int tau = 2; tau <= pacf_size; tau++)
206			{
207			// Hannan-Rissanen initial phi
208		✗	if (tau == p_approx + 1) // hand over AR parameters at matching moment
209			{
210		✗	phi_initial = phi_tau_i;
211			}
212			// PACF calculation through Durbin-Levinson
213			// skip this if phi_initial is determined:
214		✗	if (tau < p_approx + 1)
215			{
216		✗	pacf(tau - 1) = (acf(tau) - sum1) / (1 - sum2);
217		✗	sum1 = 0;
218		✗	sum2 = 0;
219
220		✗	for (int i = 0; i < tau - 1; i++)
221			{
222		✗	phi_tau(i) = (phi_tau_i(i) - pacf(tau - 1) * phi_tau_i(tau - i - 2));
223		✗	sum1 += phi_tau(i) * acf(tau - i); // numerator sum
224		✗	sum2 += phi_tau(i) * acf(i + 1); // denominator sum
225			}
226		✗	phi_tau_i = phi_tau;
227		✗	phi_tau_i(tau - 1) = pacf(tau - 1); // last element of phi_tau_i -> phi_tau_tau
228		✗	sum1 += pacf(tau - 1) * acf(1);
229		✗	sum2 += pacf(tau - 1) * acf(tau);
230			}
231
232			/* Hannan-Rissanen initial ê
233			calculate estimates of y (y_hat) for AR(p_approx)-process to determine resulting ê */
234		✗	if (tau > p_approx)
235			{
236		✗	for (int iter = 0; iter < p_approx; iter++)
237			{
238		✗	sum3 += y(tau - iter - 2) * phi_initial(iter); // y_hat of current tau
239			}
240		✗	e_hat_initial(tau - 1) = y(tau - 1) - sum3; // ê = y - y_hat
241		✗	sum3 = 0;
242			}
243			else
244			{
245		✗	e_hat_initial(tau - 1) = 0; // ê_t for t < m = 0 (condition in Yule-Walker estimation)
246			}
247			}
248		✗	}
249
250		✗	void NAV::experimental::ARMA::matrix_function(Eigen::VectorXd& y, Eigen::VectorXd& e_hat, int p, int q, int m, Eigen::MatrixXd& A)
251			{
252		✗	for (int t = m; t < y.size(); t++) // rows
253			{
254		✗	for (int i = 0; i < p; i++) // AR columns
255			{
256		✗	A(t - m, i) = y(t - i - 1); // dependency of y_t-1 ... y_t-p
257			}
258		✗	for (int i = p; i < p + q; i++) // MA columns
259			{
260		✗	A(t - m, i) = -e_hat(t - i + p - 1); // dependency of ê_t-1 ... ê_t-q
261			}
262			}
263		✗	}
264
265		✗	void NAV::experimental::ARMA::hannan_rissanen(Eigen::VectorXd& y, int p, int q, int m, int deque_size, Eigen::VectorXd& x, Eigen::VectorXd& emp_sig, Eigen::VectorXd& y_hat)
266			{
267			// declaration
268			// acf
269		✗	Eigen::VectorXd acf = Eigen::VectorXd::Zero(deque_size);
270			// pacf
271		✗	Eigen::VectorXd pacf = Eigen::VectorXd::Zero(deque_size - 1);
272		✗	Eigen::VectorXd e_hat = Eigen::VectorXd::Zero(deque_size);
273			// arma
274		✗	Eigen::MatrixXd A = Eigen::MatrixXd::Zero(deque_size - m, p + q);
275			// arma parameter test
276		✗	Eigen::VectorXd t = Eigen::VectorXd::Zero(p + q);
277		✗	Eigen::MatrixXd ata_inv = Eigen::MatrixXd::Zero(p + q, p + q); // inverse Covariance matrix of least squares estimator
278
279			// necessary for parameter test:
280		✗	int df = deque_size - p - q - 1; // degrees of freedom
281		✗	boost::math::students_t dist(df); // T distribution
282
283			// calculate acf
284		✗	acf_function(y, p, acf);
285
286			// calculate pacf & initial ê
287		✗	pacf_function(y, acf, p, pacf, e_hat);
288			// arma process
289		✗	for (int it = 0; it < 2; it++)
290			{
291		✗	matrix_function(y, e_hat, p, q, m, A); // set A
292
293		✗	x = (A.transpose() * A).ldlt().solve(A.transpose() * y.tail(deque_size - m)); // t > max(p, q)
294
295		✗	for (int i_HR = 0; i_HR < m; i_HR++) // for t <= max(p,q)
296			{
297		✗	y_hat(i_HR) = y(i_HR); // y_hat = y
298			}
299		✗	y_hat.tail(deque_size - m) = A * x; // calculate y_hat and ê for t > max(p,q)
300		✗	e_hat = y - y_hat;
301			}
302
303			// arma parameter test (parameter significance test)
304		✗	double e_square = e_hat.transpose() * e_hat;
305		✗	double var_e = e_square / df; // sigma^2_e
306		✗	ata_inv = (A.transpose() * A).inverse();
307		✗	for (int j = 0; j < p + q; j++)
308			{
309		✗	t(j) = x(j) / sqrt(var_e * ata_inv(j, j)); // t quantile
310		✗	emp_sig(j) = 2 * boost::math::pdf(dist, t(j)); // probability for two-sided t-Test
311			}
312		✗	}
313
314		✗	void NAV::experimental::ARMA::receiveImuObs(NAV::InputPin::NodeDataQueue& queue, size_t /* pinIdx */)
315			{
316		✗	auto obs = std::static_pointer_cast<const ImuObs>(queue.extract_front());
317		✗	auto newImuObs = std::make_shared<ImuObs>(obs->imuPos);
318		✗	_buffer.push_back(obs); // push latest IMU epoch to deque
319
320		✗	if (static_cast<int>(_buffer.size()) == _deque_size) // deque filled
321			{
322		✗	_k = 0;
323		✗	for (auto& obs : _buffer) // read observations from buffer to y
324			{
325		✗	const Eigen::Vector3d acc = obs->p_acceleration; // acceleration in x, y and z
326		✗	const Eigen::Vector3d gyro = obs->p_angularRate; // gyro in x, y and z
327		✗	_y.row(_k) << acc.transpose(), gyro.transpose(); // write to y
328		✗	_k++;
329			}
330		✗	for (int obs_nr = 0; obs_nr < _num_obs; obs_nr++) // build ARMA-model for each observation
331			{
332		✗	_p = _p_mem; // reset p, q to initial for next observation
333		✗	_q = _q_mem;
334
335		✗	_y_mean = _y.col(obs_nr).mean();
336		✗	_y_rbm = _y.col(obs_nr) - _y_mean * Eigen::VectorXd::Ones(_deque_size, 1); // reduce y by mean
337
338		✗	INITIALIZE = true; // set INITIALIZE true for each observation
339		✗	while (INITIALIZE) // while initializing ARMA-parameters
340			{
341		✗	if (_p + _q == 0) // arma(0,0) -> y_hat = 0
342			{
343		✗	_y_hat(_deque_size - 1) = 0;
344		✗	break;
345			}
346		✗	_x.resize(_p + _q); // resize
347		✗	_emp_sig.resize(_p + _q); // resize
348
349		✗	_m = static_cast<int>(std::max(_p, _q));
350
351		✗	hannan_rissanen(_y_rbm, _p, _q, _m, _deque_size, _x, _emp_sig, _y_hat); // parameter estimation
352
353			// zero slope parameter test: search for emp_sig(p-Value) > alpha(0.05)
354		✗	if (_emp_sig.maxCoeff() > 0.05)
355			{
356		✗	int arma_it = 0;
357		✗	for (arma_it = 0; arma_it < _p + _q; arma_it++)
358			{
359		✗	if (_emp_sig(arma_it) == _emp_sig.maxCoeff()) // find index of maximum
360			{
361		✗	break;
362			}
363			}
364		✗	if (arma_it < _p) // if p-value has maximum for phi
365			{
366		✗	_p--; // reduce AR order by 1
367			}
368			else // if p-value has maximum for theta
369			{
370		✗	_q--; // reduce MA order by 1
371			}
372			}
373			else
374			{
375		✗	INITIALIZE = false; // initialized parameters for observation
376			}
377			}
378		✗	_y_hat_t(obs_nr) = _y_hat(_deque_size - 1) + _y_mean; // hand over last entry of y_hat and add y_mean
379			}
380			// output
381			LOG_TRACE("{}: called {}", nameId(), obs->insTime.toYMDHMS());
382		✗	newImuObs->insTime = obs->insTime;
383		✗	newImuObs->p_acceleration = Eigen::Vector3d(_y_hat_t.head(3)); // output estimations of accelerometer observations
384		✗	newImuObs->p_angularRate = Eigen::Vector3d(_y_hat_t.tail(3)); // output estimations of gyro observations
385		✗	invokeCallbacks(OUTPUT_PORT_INDEX_IMU_OBS, newImuObs);
386		✗	_buffer.pop_front();
387			}
388			else // output = input while filling deque
389			{
390		✗	invokeCallbacks(OUTPUT_PORT_INDEX_IMU_OBS, obs);
391			}
392		✗	}
393