# -*- coding: utf-8 -*- """ Created on Thu Dec 8 09:55:47 2022 @author: rpivovar """ import numpy as np from twinstat.statespace_models.estimators import kalman def get_random_data(): n_data = 40 x = np.linspace(0,100,num=n_data) y_org = np.sin(50*x) #gaussian noise np.random.seed(0) y1 = y_org + np.random.normal(0, 0.5, size=n_data) y2 = y_org + np.random.normal(0, 0.5, size=n_data) y1 = np.expand_dims(y1,-1) y2 = np.expand_dims(y2,-1) y = np.concatenate((y1,y2, y2),axis=-1) return y def test_kf(): y = get_random_data() kf = kalman('kf',y) smoothed_state_means, _ = kf.get_estimate(y) good_arr = np.array([[ 0.88202617, -0.52427648, -0.52427648], [ 0.80132742, -0.25795436, -0.25795436], [ 0.01991719, -1.2201598 , -1.2201598 ], [ 1.30491001, 0.74406371, 0.74406371], [ 0.65816754, -0.29085591, -0.29085591], [ 0.03606593, -0.1597666 , -0.1597666 ], [ 0.58143324, -0.17410078, -0.17410078], [-0.36371012, -0.36526798, -0.36526798], [ 0.44476806, -0.02265254, -0.02265254], [-0.18022372, -0.55138478, -0.55138478], [ 0.14734828, -0.31564893, -0.31564893], [ 0.6993221 , 0.19263713, 0.19263713], [ 0.0110085 , -0.57554899, -0.57554899], [ 0.65901068, 0.0325306 , 0.0325306 ], [-0.13852221, -0.52368638, -0.52368638], [ 0.30346173, 0.1855929 , 0.1855929 ], [ 0.68700844, 0.20084568, 0.20084568], [-0.23480825, -0.26363955, -0.26363955], [ 0.61367234, 0.30990021, 0.30990021], [-0.59691317, -0.56110689, -0.56110689], [-0.68707879, -0.09228647, -0.09228647], [-0.03746863, -0.12336547, -0.12336547], [-0.11497449, -0.66618566, -0.66618566], [ 0.31077576, -0.20387723, -0.20387723], [ 0.22395163, -0.61919337, -0.61919337], [ 0.03601216, 0.039226 , 0.039226 ], [-0.03597393, -0.55264551, -0.55264551], [-0.36606251, -0.36256292, -0.36256292], [ 0.88373168, 0.13104297, 0.13104297], [ 0.17849235, -0.54701397, -0.54701397], [ 0.57810043, 0.47840165, 0.47840165], [ 0.1882045 , 0.07312222, 0.07312222], [-0.41779904, 0.16467851, 0.16467851], [-0.26679219, 0.18621204, 0.18621204], [-0.82738487, -0.42251148, -0.42251148], [ 0.24733283, 0.14204826, 0.14204826], [ 0.24254099, -0.44694598, -0.44694598], [ 0.33224364, -0.48154714, -0.48154714], [ 0.45711589, 0.16969101, 0.16969101], [-0.52941079, -0.5284246 , -0.5284246 ]]) compare = np.allclose(smoothed_state_means,good_arr, rtol=1e-4,atol=1e-4) assert compare == True def test_akf(): y = get_random_data() akf = kalman('akf',y, n_em_iterations=5) smoothed_state_means, _ = akf.get_estimate(y) good_arr = np.array([[ 1.02674157, -0.62973775, -0.62973775], [ 0.6067643 , -0.31721825, -0.31721825], [ 0.14721759, -0.5579493 , -0.5579493 ], [ 0.69857086, 0.27797238, 0.27797238], [ 0.21747565, -0.29512249, -0.29512249], [ 0.00564977, -0.1093216 , -0.1093216 ], [ 0.36798435, -0.06462768, -0.06462768], [-0.21216967, -0.17213172, -0.17213172], [ 0.36557281, 0.01634106, 0.01634106], [-0.03861866, -0.26216492, -0.26216492], [ 0.20941982, -0.10557748, -0.10557748], [ 0.4023296 , 0.02598831, 0.02598831], [ 0.03808049, -0.32883485, -0.32883485], [ 0.42276701, 0.02283286, 0.02283286], [-0.10137115, -0.24612158, -0.24612158], [ 0.19530462, 0.08432699, 0.08432699], [ 0.31343172, 0.01284641, 0.01284641], [-0.18969304, -0.17340024, -0.17340024], [ 0.35385041, 0.15194643, 0.15194643], [-0.39825823, -0.20375412, -0.20375412], [-0.27265006, 0.09321152, 0.09321152], [ 0.17255426, -0.03505915, -0.03505915], [ 0.11354989, -0.29005621, -0.29005621], [ 0.3251942 , -0.08326895, -0.08326895], [ 0.18552142, -0.25214836, -0.25214836], [ 0.02607591, 0.03827949, 0.03827949], [ 0.07456544, -0.20335545, -0.20335545], [-0.08670364, -0.12800389, -0.12800389], [ 0.62023877, 0.01533037, 0.01533037], [ 0.04763397, -0.30752356, -0.30752356], [ 0.22411305, 0.20274827, 0.20274827], [-0.06441584, -0.00753565, -0.00753565], [-0.34746303, 0.11170026, 0.11170026], [-0.1425507 , 0.14026785, 0.14026785], [-0.35290362, -0.14282267, -0.14282267], [ 0.33192518, 0.07579204, 0.07579204], [ 0.26147697, -0.2845393 , -0.2845393 ], [ 0.26700557, -0.20339035, -0.20339035], [ 0.18678794, 0.0867994 , 0.0867994 ], [-0.29607953, -0.20433355, -0.20433355]]) compare = np.allclose(smoothed_state_means,good_arr, rtol=1e-4,atol=1e-4) assert compare == True def test_ukf(): y = get_random_data() ukf = kalman('ukf', y) smoothed_state_means, _ = ukf.get_estimate(y) good_arr = np.array([[ 0.88202617, -0.52427648, -0.52427648], [ 0.8178052 , -0.30567006, -0.30567006], [ 0.05192212, -1.21106877, -1.21106877], [ 1.31097067, 0.73920437, 0.73920437], [ 0.66094104, -0.29207021, -0.29207021], [ 0.03718959, -0.16024232, -0.16024232], [ 0.58185423, -0.1742823 , -0.1742823 ], [-0.36354723, -0.36533695, -0.36533695], [ 0.44483001, -0.02267898, -0.02267898], [-0.18020003, -0.55139486, -0.55139486], [ 0.14735732, -0.31565278, -0.31565278], [ 0.69932556, 0.19263566, 0.19263566], [ 0.01100982, -0.57554956, -0.57554956], [ 0.65901119, 0.03253038, 0.03253038], [-0.13852202, -0.52368646, -0.52368646], [ 0.3034618 , 0.18559287, 0.18559287], [ 0.68700847, 0.20084567, 0.20084567], [-0.23480824, -0.26363955, -0.26363955], [ 0.61367234, 0.30990021, 0.30990021], [-0.59691317, -0.56110689, -0.56110689], [-0.68707879, -0.09228647, -0.09228647], [-0.03746863, -0.12336547, -0.12336547], [-0.11497449, -0.66618566, -0.66618566], [ 0.31077576, -0.20387723, -0.20387723], [ 0.22395163, -0.61919337, -0.61919337], [ 0.03601216, 0.039226 , 0.039226 ], [-0.03597393, -0.55264551, -0.55264551], [-0.36606251, -0.36256292, -0.36256292], [ 0.88373168, 0.13104297, 0.13104297], [ 0.17849235, -0.54701397, -0.54701397], [ 0.57810043, 0.47840165, 0.47840165], [ 0.1882045 , 0.07312222, 0.07312222], [-0.41779904, 0.16467851, 0.16467851], [-0.26679219, 0.18621204, 0.18621204], [-0.82738487, -0.42251148, -0.42251148], [ 0.24733283, 0.14204826, 0.14204826], [ 0.24254099, -0.44694598, -0.44694598], [ 0.33224364, -0.48154714, -0.48154714], [ 0.45711589, 0.16969101, 0.16969101], [-0.52941079, -0.5284246 , -0.5284246 ]]) compare = np.allclose(smoothed_state_means,good_arr, rtol=1e-4,atol=1e-4) assert compare == True