-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathkalman_energy_estimation.py
210 lines (201 loc) · 17.1 KB
/
kalman_energy_estimation.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
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
from filterpy.kalman import KalmanFilter
import numpy as np
from filterpy.common.discretization import Q_discrete_white_noise
import matplotlib.pyplot as plt
e = [0.49910644235205126, 0.49829546759900134, 0.49755899386231683, 0.49666919119498976, 0.4957101216693384,
0.49493112572797016, 0.49418856809559264, 0.49265396394746086, 0.49185221570924403, 0.49107459479472304,
0.4902329232777474, 0.48943181015432907, 0.48860545045581655, 0.4877487444449937, 0.48700154744219054,
0.48615824921035705, 0.4853346358462779, 0.4843987491358902, 0.483597455860384, 0.48283302925985955,
0.48196111282063947, 0.48118655078039263, 0.48041099276976285, 0.4796956426076996, 0.47876182899813746,
0.4778873795122322, 0.47693357098767847, 0.47614817412105803, 0.47535237379892686, 0.47432906063424324,
0.4734807796362579, 0.4726079890075735, 0.47185384886269, 0.47080713867036755, 0.4699903147532597,
0.46922112255356896, 0.46848628083670385, 0.4676832008875929, 0.46693777496788597, 0.46610452735790403,
0.4652368206058153, 0.4644887457050939, 0.4633249600080177, 0.4621647479009996, 0.46128246431210657,
0.46046007285004315, 0.4594485288123902, 0.4580222299554404, 0.45712886504521627, 0.45633632819137615,
0.4555160686499533, 0.45463626637595, 0.45383260552552307, 0.4526185962718291, 0.4516686722314687,
0.45072423006933554, 0.4499025708515024, 0.4491316970408187, 0.44840357571202105, 0.4476566794348906,
0.44682497901912016, 0.4455100724256653, 0.4445815026203201, 0.4436847120609223, 0.4429096796347485,
0.44220581160072525, 0.44080409684154154, 0.43998820561552804, 0.4391505711370938, 0.43843545370886877,
0.4375672856145498, 0.4367882258943711, 0.43588427431250737, 0.4349452818696982, 0.43412696567897746,
0.43327935711303106, 0.43248077940394397, 0.43173236367421586, 0.43092471377146124, 0.42998510572867843,
0.42923406550435184, 0.42851688705946733, 0.4276868689062183, 0.4268665558054402, 0.42602524824152405,
0.42508546026492305, 0.42436214699614316, 0.4229724140913622, 0.42207578535344864, 0.42129417303241196,
0.4204625003987272, 0.41967084018708134, 0.4188940174738377, 0.41801996096363203, 0.4166430472069885,
0.4158784175840344, 0.4150917941046044, 0.41285314964500924, 0.412097698263535, 0.41133861313425085,
0.41060127857975187, 0.4098045365028895, 0.4090506195026227, 0.4082882816979028, 0.4074830047850612,
0.4066430016114539, 0.40569094221155066, 0.4048552034093192, 0.4040315972177942, 0.40323854384560365,
0.4023538603636889, 0.4015835276632723, 0.40080622557285767, 0.39996244717606466, 0.3992157392906423,
0.3984086992121428, 0.3975809128181453, 0.3967890218322958, 0.39597430301050407, 0.39510895204183627,
0.39431261122294103, 0.393493894107139, 0.3926847087783657, 0.39169854709964325, 0.3909063189841768,
0.38693338986774956, 0.3861965985641184, 0.3853618290193056, 0.38446312213634093, 0.38370076794151714,
0.38288726263918527, 0.38207560477067654, 0.38126331174911293, 0.38054127070180926, 0.3797375187071511,
0.3789701217185988, 0.37794667071338844, 0.37714626068566093, 0.3763964998198285, 0.3744518568092983,
0.37355655383550457, 0.3726511725690495, 0.3717461890578585, 0.370768797612484, 0.369634454447539,
0.36890931393584814, 0.3681252042202021, 0.3672471541027273, 0.36624274877803376, 0.3654309789160232,
0.36462984331716597, 0.3638689771741519, 0.3630889053545012, 0.3623432280401487, 0.3615946692327896,
0.3606849539859732, 0.35982632565345674, 0.3590339301964168, 0.35821216617156354, 0.35746270127072177,
0.3559121490013094, 0.35505517672004916, 0.35388317115171686, 0.3504952167113348, 0.3497675708486225,
0.3487804353552058, 0.3480161913862853, 0.34723054685092203, 0.34644368719174345, 0.3456585790350664,
0.34485014610681397, 0.3437320986706382, 0.34302562871325953, 0.34216964982968767, 0.34139928509921896,
0.34058031962282337, 0.33981344213443565, 0.339002001284733, 0.3382602424663408, 0.3375460077580608,
0.33628345964867273, 0.3354455368447543, 0.3346676991514243, 0.3338218532521692, 0.3330127561731953,
0.33226046590501085, 0.3315130084409416, 0.3306654819366169, 0.3297541288562173, 0.32898753645930456,
0.3280238559169857, 0.32719452393766035, 0.32629180340703234, 0.32548617713827743, 0.3246824033430162,
0.3238969459160569, 0.3229412046966146, 0.3220262785527887, 0.321147445319447, 0.320419464431549,
0.3195921788706489, 0.31885991401483477, 0.3180927296442089, 0.31726041704907004, 0.3164353275830548,
0.31567859375614526, 0.31491287880550844, 0.3141329666902692, 0.3132293615413118, 0.3112484199500388,
0.3105014220996417, 0.309729482367191, 0.3089511251035514, 0.30813424726726396, 0.30482183794679896,
0.30411049549805275, 0.3032975106957832, 0.3024574498096775, 0.3016840318610974, 0.30085571824794094,
0.2999164989352202, 0.2990560003055951, 0.29822981388462966, 0.2974821982613123, 0.2967032855232324,
0.29587427282035206, 0.29485875531879807, 0.29379537321947774, 0.29291384227572903, 0.2920352341010627,
0.29121900026074, 0.29046497220216067, 0.2896745919542344, 0.2888018064257027, 0.2879476309915309,
0.2872058492284661, 0.28641851692025594, 0.28541937304807374, 0.2846574302348927, 0.28384914884230616,
0.283066282193529, 0.2822768169285666, 0.2813972083635108, 0.28046972900921613, 0.2797484227864985,
0.27865209295759347, 0.2778367998118057, 0.277033568573683, 0.2762591810507141, 0.27532813818882995,
0.2743298225782295, 0.2736070955963516, 0.27285821056234977, 0.27207468970339554, 0.27118574806023327,
0.2704329720106179, 0.26943848883288846, 0.26862152088667246, 0.2678254377327325, 0.2669372989564198,
0.2661513143096833, 0.26532899189637477, 0.2644443538015539, 0.26358217982795645, 0.26279422141916786,
0.26190010494443305, 0.2611400599058356, 0.260348590919744, 0.25934063832816134, 0.2585396031886317,
0.2577294284828854, 0.25699430885376573, 0.25628925683644854, 0.25550782244178327, 0.2547523749502715,
0.25385750472504826, 0.2530374605665679, 0.2522021537642632, 0.2514245162413197, 0.25067776245541495,
0.24982942590931623, 0.24898354672838535, 0.24824242881474048, 0.2472855970739108, 0.2463635528099641,
0.24529261865748034, 0.244500871448394, 0.24351240470492239, 0.2425307737275149, 0.241799163994681,
0.24101405434596807, 0.2401210292664303, 0.2393160856258933, 0.23855124811031875, 0.2376658418240619,
0.23679014098459583, 0.23598919168687035, 0.23520992343851943, 0.2343738323005825, 0.2335451639794247,
0.23272170043375176, 0.231992820922736, 0.23103894859625565, 0.23014879820292797, 0.2293462382911866,
0.22847735357318602, 0.22770278775708142, 0.2269403473553324, 0.22615477120952046, 0.22528889200190638,
0.22447600733663564, 0.2236912283884413, 0.2226553998457522, 0.22187912283849395, 0.22113607385422682,
0.21984017766206226, 0.21893481549767946, 0.2181629051877224, 0.2173952764378653, 0.21661466220632483,
0.21584933169576012, 0.21496411524029277, 0.21404474750391494, 0.2132717066962351, 0.21200216727954888,
0.2112835855900117, 0.21046524841049696, 0.2097365949778868, 0.20892309346819235, 0.2081435952818831,
0.20722813734364245, 0.20644417652837718, 0.2055701533753675, 0.20473530919312044, 0.20390953093207514,
0.2030827327958917, 0.20205971012946652, 0.20104220428397115, 0.20003799362323552, 0.199211776430617,
0.19845347839060604, 0.19773485404724556, 0.19676138548325534, 0.1959281797696302, 0.19515986471950203,
0.19426921325720317, 0.19351949569524635, 0.19281087458130042, 0.19206957360506788, 0.1913022010104481,
0.1902456547416164, 0.18950208177610542, 0.18873906966274048, 0.18798247647288002, 0.18718716057457432,
0.18642998313409398, 0.18563286652702396, 0.1848690766694875, 0.18383830025639494, 0.18307108529828217,
0.18236345830101958, 0.18148009385462857, 0.1805576721189216, 0.1798710828420863, 0.1788249064241165,
0.17805994008966894, 0.17731735527162987, 0.17657460958102625, 0.17582581367784286, 0.17498567909940427,
0.17413533217865115, 0.17337826955469265, 0.17268232078371262, 0.1720016517368535, 0.17115128604984264,
0.17031909782821075, 0.16960238230022182, 0.16885275087458837, 0.16807288216653213, 0.16696363118408858,
0.16616583992448958, 0.16536870324886838, 0.16466350703607915, 0.16383769545663568, 0.1630919188175119,
0.16229692821944855, 0.1615118790270199, 0.1607464753240804, 0.15988347208577733, 0.15899409098261472,
0.15825360727725646, 0.1575576436819833, 0.15686139769657662, 0.15587228315481427, 0.1551129022698002,
0.15406511041057672, 0.15337452702509055, 0.15266914616298546, 0.15196462064885227, 0.15131127510474388,
0.15059093873500845, 0.14986161927859562, 0.14920080625430934, 0.14816443071289379, 0.14745397887381595,
0.1466583964191932, 0.14595769771266112, 0.14526525001957605, 0.14278366124717626, 0.14212578860685282,
0.1398132962285856, 0.13919041901325632, 0.1384834868109574, 0.1378043611520553, 0.13713147746938656,
0.13644242056679048, 0.13577829351389412, 0.13475147831400883, 0.13405952858106773, 0.13337150070053266,
0.1326541255373034, 0.1318828007604485, 0.13119071791117157, 0.1305213440451357, 0.1297965503973716,
0.12913610122386854, 0.12837732892015544, 0.1277153108659357, 0.12703967622521076, 0.1262994222572372,
0.1255178845027578, 0.12479752208187676, 0.12401613325401364, 0.1233536492546225, 0.12249435275160626,
0.12172212885779016, 0.12104356554842395, 0.11932253894110467, 0.11859156822603215, 0.11788676051247844,
0.11725845169381811, 0.11660684647491641, 0.11591961759912466, 0.11526298754429842, 0.11461283074462343,
0.11244752697124902, 0.11179094477329508, 0.11109570636659567, 0.11044712619598973, 0.1098134148743096,
0.10919639093759273, 0.10838004795792658, 0.10770383245607111, 0.10704683047766181, 0.10632433824847735,
0.1056484276611972, 0.10493359008775066, 0.10390478836835508, 0.10327450631525055, 0.1024817508131784,
0.10184385245140222, 0.10104600757086271, 0.10040846766244839, 0.09977272764285562, 0.0991334840661483,
0.09852203902261887, 0.09774784369266998, 0.0971438934864939, 0.09647447299854538, 0.09591111122523309,
0.09519282462800831, 0.09450310791787332, 0.09389951727518961, 0.09321640327387452, 0.09258318821672536,
0.09195153436605649, 0.0913704495411761, 0.09061357079268588, 0.08991712849978313, 0.08919635505994059,
0.08860700675950682, 0.08801442176749452, 0.08722538673935963, 0.08654058487182505, 0.08588466361940057,
0.08527254256189344, 0.0845426654848109, 0.0839593052184377, 0.08335524538414005, 0.0824446943723886,
0.08189715895926264, 0.08116173383400849, 0.08060614959395318, 0.07997116164405353, 0.07940224154074527,
0.07878260504929424, 0.07789676635724894, 0.07731285792856596, 0.07664578123420002, 0.07598711994590998,
0.07548758266232075, 0.07496833834488398, 0.07424660836784569, 0.07370392546845382, 0.07303577295755546,
0.07244155485521861, 0.07189240635098987, 0.07128656629556833, 0.07075089536062598, 0.06997816735929425,
0.06945844510518985, 0.06891624310837241, 0.06825507363703466, 0.06770833726581646, 0.06711753275521709,
0.06652027846080458, 0.06594928705661521, 0.06542742062421474, 0.06475884421479142, 0.06420475212830155,
0.06370383340370894, 0.06316041404261268, 0.06257355861743301, 0.061850708992887984, 0.06135574448480553,
0.06080810568653325, 0.06030083609169352, 0.059759876419858866, 0.05917184571523663, 0.05815114932497252,
0.05760932938711415, 0.057099657326604276, 0.05642822097412837, 0.05591206774133754, 0.05539057812186399,
0.0546333342620901, 0.05411273063892036, 0.05317788184237982, 0.0526747555858062, 0.05219207258692547,
0.05167109167832384, 0.051084098930506174, 0.05042147094578761, 0.04990031902854329, 0.04939733401650814,
0.048739655809212336, 0.048198444444225545, 0.04768671040557301, 0.04715777797842132, 0.04665094180493013,
0.046184989909890464, 0.04567465367467345, 0.04517561841388547, 0.04462002966699437, 0.044061660324707284,
0.0430602440371444, 0.042498653530338756, 0.041965629739634774, 0.04144751299473662, 0.0406539057468516,
0.0401282186989408, 0.039644468616235176, 0.03919121265256115, 0.038680173003615696, 0.03822617905068027,
0.03771936973876945, 0.03725083765159797, 0.036506572215815665, 0.03603099000322445, 0.03539836382463788,
0.03496268532438931, 0.034491367188294514, 0.03406579407747555, 0.03362943352531774, 0.03321819414107584,
0.03269799397397482, 0.03225907601097938, 0.0318079031330796, 0.03137669788673954, 0.030982691020150933,
0.03059826306652106, 0.029692678580603572, 0.029330202559745867, 0.028706815014913324, 0.02799278342834273,
0.027560103110230452, 0.027234818399671866, 0.026906280000735663, 0.02653005000209748, 0.02618854860489387,
0.025806227095424093, 0.025417911826167504, 0.025098826852814345, 0.024786481486280366, 0.024455127056904093,
0.023689475924354393, 0.023405452069276637, 0.023048656545126618, 0.02274995794031063, 0.022480042501888457,
0.02221103871595572, 0.02176806789523245, 0.02146556450698403, 0.021159454682403624, 0.020886088693858967,
0.020687615550890347, 0.020413207686629863, 0.02010314482458927, 0.019702954109600962, 0.019434279918124516,
0.018796549638185087, 0.01820408483859096, 0.017923840543944115, 0.01770375718230839, 0.017539458015812477,
0.017323474912474955, 0.017121533315063667, 0.016907464479821086, 0.016705197006710897, 0.016452492607841766,
0.01625343595528628, 0.016020100188019564, 0.0158204485109376, 0.015613283982687052, 0.01532794094738641,
0.015192314337651032, 0.015017425824009303, 0.014848713723011546, 0.014669742403482441, 0.014499940013035057,
0.014252588621923896, 0.014097325430855363, 0.013908189405774603, 0.013592898552010048, 0.013452943661354338,
0.013350366427014976, 0.013223468310474546, 0.013119223560799332, 0.013006600742449935, 0.01288112376053516,
0.01278753263880684, 0.012670005841790202, 0.012557747045257937, 0.012454722388680001, 0.01236065303179492,
0.012255811101786118, 0.01216827478578949, 0.012058327735758505, 0.011965910313869003, 0.011857839971025299,
0.011764452643408124, 0.011667731286503875, 0.011575677367363623, 0.011478633392265857, 0.011401248988895949,
0.011321672099734363, 0.011234141804221396, 0.011150399763229285, 0.011053257715618962, 0.0109670840337643,
0.010848908511967665, 0.010773745604682297, 0.010685849657387033, 0.010614652269568179, 0.01052979555255186,
0.010448388328622852, 0.010375601587335383, 0.010312682722799766, 0.010230676488493478, 0.010161196969989216,
0.010088453918426981, 0.010023978995083276, 0.009957855338762665, 0.009904305280757295, 0.009853775139411072,
0.009802444662327317, 0.009770541771841286, 0.009722538103306897, 0.009677145260764863, 0.009631051602756527,
0.009584477882492629, 0.009542927184738856, 0.009495552187831952, 0.009458245989716443, 0.009412070784076217,
0.009365497063812319, 0.00931749339527793, 0.00927906518831472, 0.009263054891801702, 0.009243811139419982,
0.009226398524658078, 0.009208286677124918, 0.009189042924743198, 0.009174196139531791, 0.009164628251654311,
0.009154854877325622, 0.009145081502996932, 0.009135513615119452, 0.00912574024079076, 0.009116172352913282,
0.009106604465035804, 0.009096831090707112, 0.009087057716378421, 0.009077489828500943, 0.009067716454172252,
0.009058148566294772, 0.009048580678417293, 0.009038807304088603, 0.009034216835995965, 0.00902962636790333,
0.009025035899810692, 0.009020445431718056, 0.00901585496362542, 0.009011264495532783, 0.009006674027440147,
0.009002083559347509, 0.008997493091254872, 0.008992902623162236, 0.0089883121550696, 0.008983721686976961,
0.008979131218884327, 0.008974540750791689, 0.008969950282699053, 0.008965359814606416, 0.00896076934651378,
0.008956178878421144, 0.008951588410328505, 0.008946997942235869, 0.008942407474143233, 0.008937817006050596,
0.008933226537957958, 0.008928636069865324, 0.008926879799783216]
e = np.array(e)
period = 20
r_warm = 2
kf = KalmanFilter(dim_x=2, dim_z=1, dim_u=0)
kf.x = np.array([
[e[1]],
[(e[1] - e[0]) * period]
])
kf.F = np.array([
[1, 1],
[0, 1],
])
# kf.B = np.array([[-1]])
kf.H = np.array([[1, 0]])
kf.P *= 1000
kf.R = 0
kf.Q *= Q_discrete_white_noise(2, 1, .1)
e_prev = kf.x[0][0]
kf.predict()
de = kf.x[1][0]
e_hat = np.array([e[0]])
for i in range(1, len(e)):
j = i - 1
r, m = divmod(j, period)
if m == 0:
e_prev = kf.x[0][0]
kf.update(e[i])
kf.predict()
de = kf.x[1][0]
if r < r_warm:
e_hat = np.append(e_hat, e[i])
else:
ee = de / period * m + e_prev
e_hat = np.append(e_hat, ee)
error = np.mean(np.abs(e_hat - e) / np.mean(e))
print(f"{error:.2%}")
with plt.style.context(["science", "ieee", "grid", "no-latex", "cjk-sc-font"]):
fig, ax = plt.subplots()
ax.plot(e, label="LEACH", color="black", zorder=0)
for i, x in enumerate(e_hat):
j = i - 1
if j % period == 0:
ax.scatter(i, x, marker="+", s=10, linewidths=0.5, color="red", zorder=1)
# ax.legend(title="protocols")
ax.set(xlabel="轮数")
ax.set(ylabel="平均能量")
# ax.legend()
ax.autoscale(tight=True)
fig.savefig("kalman_estimation.png", dpi=300)
# plt.show()