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