-
Notifications
You must be signed in to change notification settings - Fork 0
/
kalplots.py
175 lines (155 loc) · 5.76 KB
/
kalplots.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
# -*- coding: utf-8 -*-
"""
Created on Sun Nov 8 18:34:27 2020
@author: cognotrend
"""
import numpy as np
import matplotlib.pyplot as plt
def collate(v1,v2):
# collate.m collates two vectors of length m into a single vector of
# length 2m.
if v1.shape==v2.shape:
(m,)=v1.shape
collated = np.empty((2*m,),dtype='object')
# collated = collated.astype(type(v1[0]))
j=0
for i in range(m):
collated[j]=v1[i]
collated[j+1]=v2[i]
j=j+2
else:
collated=-1
return collated
def new_collate(v1,v2):
if v1.shape==v2.shape:
(n,m)=v1.shape
tmp = np.empty((2,n),dtype='object')
tmp[0,:]=np.transpose(v1[:,0])
tmp[1,:]=np.transpose(v2[:,0])
tmp=np.transpose(tmp)
collated=tmp.reshape((2*n,1))
return collated
else:
return -1
def std_sawtooth_plot(kfobj,fignum=1,expflag=0,last_percent=0.10,
title_prefix=''): #first three state variables
plt.style.use('seaborn')
plt.figure(fignum)
lastnum=int(kfobj.numruns*last_percent)
start = kfobj.numruns - lastnum
v1 = np.zeros((3,kfobj.numruns-start))
v2 = np.zeros((3,kfobj.numruns-start))
sawtooth = np.zeros((3,2*(kfobj.numruns-start)))
for i in [0,1,2]:
v1[i,:] = kfobj.P_minus_cum[i,i,-lastnum:kfobj.numruns].reshape(lastnum,)
v2[i,:] = kfobj.P_plus_cum[i,i,-lastnum:kfobj.numruns].reshape(lastnum,)
doubletime=collate(np.arange(0,lastnum),np.arange(0,lastnum))
sawtooth[i,:]=collate(v1[i,:],v2[i,:])
if expflag==1:
sawtooth=np.exp(2*sawtooth)
ax0=plt.subplot(3,1,1)
plt.plot(doubletime,sawtooth[0,:])
plt.setp(ax0.get_xticklabels(), visible=False)
plt.title(title_prefix+' Covariance')
ax1=plt.subplot(3,1,2,sharex=ax0)
plt.plot(doubletime,sawtooth[1,:])
plt.setp(ax1.get_xticklabels(), visible=False)
ax2=plt.subplot(3,1,3,sharex=ax1)
plt.plot(doubletime,sawtooth[2,:])
plt.xlabel('Epochs')
plt.tight_layout()
plt.show()
def plot_residuals(kfobj,fignum=2,
expflag=0,
title_prefix='',
legend_str = ['']):
data1 = kfobj.zhat[:,0,:]
data2 = kfobj.z[:,0,:]
data3 = kfobj.residual[:,0,:]
suffix=''
if expflag==1:
data1 = np.exp(data1)
data2 = np.exp(data2)
data3 = kfobj.exp_residual[:,0,:]
suffix = ' (Exp)'
plt.style.use('seaborn')
epochs=list(range(0,kfobj.numruns))
if kfobj.meas_size==1:
fig, axs = plt.subplots(nrows=kfobj.meas_size,ncols=1, sharex=True)
myfig=[]
myfig.append(fig)
myaxs=[]
myaxs.append(axs)
else:
myfig, myaxs = plt.subplots(nrows=kfobj.meas_size,ncols=1, sharex=True)
for i in list(range(0,kfobj.meas_size)):
if i==0:
myaxs[i].set_title('Filter Residuals for '+kfobj.filter_id)
myaxs[i].plot(epochs[1:kfobj.numruns],data3[i,1:kfobj.numruns],
label='Residuals: '+str(i))
myaxs[i].legend(loc='upper left')
plt.tight_layout()
plt.show()
# ax1=plt.subplot(3,1,2,sharex=ax2)
# plt.setp(ax1.get_xticklabels(), visible=False)
# plt.plot(epochs[1:kfobj.numruns],data2.reshape((kfobj.numruns,))[1:kfobj.numruns])
# plt.title(title_prefix+'Actual Measurements'+suffix,fontsize=10)
# plt.show()
# plt.figure(fignum+1)
# plt.subplot(3,1,1)
# plt.hist(np.transpose(kfobj.residual),bins=100)
# plt.title(title_prefix+'Residuals Histogram')
# plt.subplot(3,1,2)
# plt.hist(np.exp(np.transpose(kfobj.residual)),bins=100)
# plt.title('Exponential of Residuals')
# plt.subplot(3,1,3)
# mu=np.mean(np.transpose(kfobj.exp_residual))
# mu=round(mu,2)
# sigma=np.std(np.transpose(kfobj.exp_residual))
# sigma=round(sigma,2)
# plt.hist(np.transpose(kfobj.exp_residual),bins=100,label=r'$\mu$=' + str(mu)+ ', $\sigma$='+str(sigma))
# plt.legend()
# plt.title('Residuals of Exponentials')
# plt.show()
def plot_gains(kfobj,state=0,fignum=4):
plt.style.use('seaborn')
epochs=np.arange(kfobj.numruns)
fig, axs = plt.subplots(nrows=kfobj.state_size,ncols=1, sharex=True)
for i in list(range(0,kfobj.state_size)):
gains = kfobj.K_cum[i,0,:].transpose()
axs[i].plot(epochs[1:kfobj.numruns],gains[1:kfobj.numruns],
label='Gain: State '+str(i)+',Meas '+str(0))
axs[i].legend(loc='upper left')
if i==0:
axs[i].set_title('Kalman Gains for each state')
plt.tight_layout()
plt.show()
def plot_states(kfobj,state=0,fignum=4,expflag=0):
epochs=np.arange(kfobj.numruns)
fig, axs = plt.subplots(nrows=kfobj.state_size,ncols=1, sharex=True)
if expflag==1:
meas = np.exp(kfobj.z[0,0,:].transpose())
else:
meas = kfobj.z[0,0,:].transpose()
for i in list(range(0,kfobj.state_size)):
if expflag==1:
states = np.exp(kfobj.x_minus[i,:].transpose())
else:
states = kfobj.x_minus[i,:].transpose()
axs[i].plot(epochs[1:kfobj.numruns],states[1:kfobj.numruns],
label='State '+str(i))
if i==0:
axs[i].plot(epochs[1:kfobj.numruns],meas[1:kfobj.numruns],
label='Measurement '+str(i))
axs[i].legend(loc='upper left')
if i==0:
axs[i].set_title('State Estimates for '+kfobj.filter_id)
plt.tight_layout()
plt.show()
def plot_posgains(kfobj,fignum=3, expflag=0):
plt.figure(fignum)
data = kfobj.posgains
if expflag==1:
data = np.exp(data)
plt.plot(data)
plt.title('Positive Gains')