Skip to content

Commit e6c8841

Browse files
mehhdiiimehhdiii
committed
with non-additive noise process.
make noise a parameter of the state and measurement functions directly
1 parent 5a3f841 commit e6c8841

File tree

5 files changed

+599
-319
lines changed

5 files changed

+599
-319
lines changed

‎Extended-Kalman-Filter-Algorithm.mltbx

24 Bytes
Binary file not shown.

‎Extended-Kalman-Filter-Algorithm.prj

+4-20
Original file line numberDiff line numberDiff line change
@@ -13,32 +13,16 @@
1313
<param.products.id />
1414
<param.products.version />
1515
<param.platforms />
16-
<param.guid>cedafdb9-6a59-4c93-a3b5-ddf093b20511</param.guid>
17-
<param.exclude.filters>% List files contained in your toolbox folder that you would like to exclude
18-
% from packaging. Excludes should be listed relative to the toolbox folder.
19-
% Some examples of how to specify excludes are provided below:
20-
%
21-
% A single file in the toolbox folder:
22-
.git
16+
<param.guid>d358422b-cb2d-4baa-ba50-a173a644611f</param.guid>
17+
<param.exclude.filters>.git
2318
simulation
2419
.gitignore
2520
LICENSE
2621
Report
2722
.gitattributes
2823
README.md
29-
%
30-
% A single file in a subfolder of the toolbox folder:
31-
% example/.svn
32-
%
33-
% All files in a subfolder of the toolbox folder:
34-
% example/*
35-
%
36-
% All files of a certain name in all subfolders of the toolbox folder:
37-
% **/.svn
38-
%
39-
% All files matching a pattern in all subfolders of the toolbox folder:
40-
% **/*.bak
41-
%</param.exclude.filters>
24+
demos.xml
25+
html</param.exclude.filters>
4226
<param.exclude.pcodedmfiles>true</param.exclude.pcodedmfiles>
4327
<param.examples />
4428
<param.demosxml />

‎demos.xml

+17
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
<?xml version="1.0" encoding="utf-8"?>
2+
<demos><!--This is an autogenerated file, please do not modify-->
3+
<name>Extended-Kalman-Filter-Algorithm</name>
4+
<type>toolbox</type>
5+
<icon>HelpIcon.DEMOS</icon>
6+
<website/>
7+
<description/>
8+
<demosection>
9+
<label>Extended-Kalman-Filter-Algorithm</label>
10+
<demoitem>
11+
<label>ExtendedKF</label>
12+
<type>other</type>
13+
<source>ExtendedKF</source>
14+
<file>html/ExtendedKF.html</file>
15+
</demoitem>
16+
</demosection>
17+
</demos>

‎html/ExtendedKF.html

+279
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,279 @@
1+
2+
<!DOCTYPE html
3+
PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
4+
<html><head>
5+
<meta http-equiv="Content-Type" content="text/html; charset=utf-8">
6+
<!--
7+
This HTML was auto-generated from MATLAB code.
8+
To make changes, update the MATLAB code and republish this document.
9+
--><title>ExtendedKF</title><meta name="generator" content="MATLAB 9.10"><link rel="schema.DC" href="http://purl.org/dc/elements/1.1/"><meta name="DC.date" content="2021-08-14"><meta name="DC.source" content="ExtendedKF.m"><style type="text/css">
10+
html,body,div,span,applet,object,iframe,h1,h2,h3,h4,h5,h6,p,blockquote,pre,a,abbr,acronym,address,big,cite,code,del,dfn,em,font,img,ins,kbd,q,s,samp,small,strike,strong,tt,var,b,u,i,center,dl,dt,dd,ol,ul,li,fieldset,form,label,legend,table,caption,tbody,tfoot,thead,tr,th,td{margin:0;padding:0;border:0;outline:0;font-size:100%;vertical-align:baseline;background:transparent}body{line-height:1}ol,ul{list-style:none}blockquote,q{quotes:none}blockquote:before,blockquote:after,q:before,q:after{content:'';content:none}:focus{outine:0}ins{text-decoration:none}del{text-decoration:line-through}table{border-collapse:collapse;border-spacing:0}
11+
12+
html { min-height:100%; margin-bottom:1px; }
13+
html body { height:100%; margin:0px; font-family:Arial, Helvetica, sans-serif; font-size:10px; color:#000; line-height:140%; background:#fff none; overflow-y:scroll; }
14+
html body td { vertical-align:top; text-align:left; }
15+
16+
h1 { padding:0px; margin:0px 0px 25px; font-family:Arial, Helvetica, sans-serif; font-size:1.5em; color:#d55000; line-height:100%; font-weight:normal; }
17+
h2 { padding:0px; margin:0px 0px 8px; font-family:Arial, Helvetica, sans-serif; font-size:1.2em; color:#000; font-weight:bold; line-height:140%; border-bottom:1px solid #d6d4d4; display:block; }
18+
h3 { padding:0px; margin:0px 0px 5px; font-family:Arial, Helvetica, sans-serif; font-size:1.1em; color:#000; font-weight:bold; line-height:140%; }
19+
20+
a { color:#005fce; text-decoration:none; }
21+
a:hover { color:#005fce; text-decoration:underline; }
22+
a:visited { color:#004aa0; text-decoration:none; }
23+
24+
p { padding:0px; margin:0px 0px 20px; }
25+
img { padding:0px; margin:0px 0px 20px; border:none; }
26+
p img, pre img, tt img, li img, h1 img, h2 img { margin-bottom:0px; }
27+
28+
ul { padding:0px; margin:0px 0px 20px 23px; list-style:square; }
29+
ul li { padding:0px; margin:0px 0px 7px 0px; }
30+
ul li ul { padding:5px 0px 0px; margin:0px 0px 7px 23px; }
31+
ul li ol li { list-style:decimal; }
32+
ol { padding:0px; margin:0px 0px 20px 0px; list-style:decimal; }
33+
ol li { padding:0px; margin:0px 0px 7px 23px; list-style-type:decimal; }
34+
ol li ol { padding:5px 0px 0px; margin:0px 0px 7px 0px; }
35+
ol li ol li { list-style-type:lower-alpha; }
36+
ol li ul { padding-top:7px; }
37+
ol li ul li { list-style:square; }
38+
39+
.content { font-size:1.2em; line-height:140%; padding: 20px; }
40+
41+
pre, code { font-size:12px; }
42+
tt { font-size: 1.2em; }
43+
pre { margin:0px 0px 20px; }
44+
pre.codeinput { padding:10px; border:1px solid #d3d3d3; background:#f7f7f7; }
45+
pre.codeoutput { padding:10px 11px; margin:0px 0px 20px; color:#4c4c4c; }
46+
pre.error { color:red; }
47+
48+
@media print { pre.codeinput, pre.codeoutput { word-wrap:break-word; width:100%; } }
49+
50+
span.keyword { color:#0000FF }
51+
span.comment { color:#228B22 }
52+
span.string { color:#A020F0 }
53+
span.untermstring { color:#B20000 }
54+
span.syscmd { color:#B28C00 }
55+
span.typesection { color:#A0522D }
56+
57+
.footer { width:auto; padding:10px 0px; margin:25px 0px 0px; border-top:1px dotted #878787; font-size:0.8em; line-height:140%; font-style:italic; color:#878787; text-align:left; float:none; }
58+
.footer p { margin:0px; }
59+
.footer a { color:#878787; }
60+
.footer a:hover { color:#878787; text-decoration:underline; }
61+
.footer a:visited { color:#878787; }
62+
63+
table th { padding:7px 5px; text-align:left; vertical-align:middle; border: 1px solid #d6d4d4; font-weight:bold; }
64+
table td { padding:7px 5px; text-align:left; vertical-align:top; border:1px solid #d6d4d4; }
65+
66+
67+
68+
69+
70+
</style></head><body><div class="content"><pre class="codeinput"><span class="keyword">classdef</span> ExtendedKF &lt; handle
71+
<span class="keyword">properties</span>
72+
73+
predhistory
74+
truehistory
75+
Plast
76+
statetransitionfcn
77+
measurementfcn
78+
statej
79+
measurementj
80+
statecovariance
81+
measurementcovariance
82+
T
83+
xk
84+
k
85+
vk <span class="comment">%plant noise</span>
86+
wk <span class="comment">%measurement noise</span>
87+
state_dim
88+
measurement_dim
89+
hasadditivenoise
90+
measurementhistory
91+
<span class="keyword">end</span>
92+
93+
<span class="keyword">methods</span>
94+
95+
<span class="comment">%constructor of class</span>
96+
<span class="keyword">function</span> self = ExtendedKF(statetransition_f, measurement_f,<span class="keyword">...</span>
97+
state_j, measurement_j, state_covariance,<span class="keyword">...</span>
98+
measurement_covariance, sampling_time, initial_x, additivenoise)
99+
<span class="keyword">if</span> nargin == 9
100+
self.statetransitionfcn = statetransition_f;
101+
self.measurementfcn = measurement_f;
102+
self.statej = state_j;
103+
self.measurementj = measurement_j;
104+
self.statecovariance = state_covariance;
105+
self.measurementcovariance = measurement_covariance;
106+
self.T = sampling_time;
107+
108+
self.state_dim = size(self.statecovariance(:, 1));
109+
self.measurement_dim = size(self.measurementcovariance(:, 1));
110+
self.truehistory(:, 1) = initial_x;
111+
self.predhistory = zeros(self.state_dim);
112+
self.measurementhistory = zeros(self.measurement_dim);
113+
self.k = 1;
114+
self.hasadditivenoise = additivenoise;
115+
self.Plast = eye(self.state_dim(1));
116+
<span class="keyword">end</span>
117+
118+
<span class="keyword">end</span>
119+
120+
121+
<span class="keyword">function</span> [Xpred, Ppred] = predict(self)
122+
<span class="comment">%create plant noise</span>
123+
self.vk = sqrt(self.statecovariance)*randn(self.state_dim(1), 1);
124+
<span class="comment">%create noisy plant state: REFERENCE VALUE</span>
125+
xtrue_last = self.predhistory(:, end);
126+
127+
xtrue = self.statetransitionfcn(xtrue_last, self.T, 0);
128+
self.xk = self.statetransitionfcn(xtrue_last, self.T, self.vk);
129+
130+
xhat_last = self.predhistory(:, end);
131+
F = self.statej(xhat_last, self.T);
132+
Xpred = self.statetransitionfcn(xhat_last, self.T, 0);
133+
Ppred = F*(self.Plast)*F' + self.statecovariance;
134+
135+
<span class="comment">%save the new values:</span>
136+
self.k = self.k+1;
137+
self.Plast = Ppred;
138+
self.predhistory(:, self.k) = Xpred;
139+
self.truehistory(:, self.k) = xtrue;
140+
141+
142+
<span class="keyword">end</span>
143+
144+
<span class="keyword">function</span> [Xcorr, Pcorr] = correct(self)
145+
<span class="comment">%create measurement noise</span>
146+
self.wk = sqrt(self.measurementcovariance)*randn(self.measurement_dim(1), 1);
147+
<span class="comment">%create true measurement</span>
148+
yk = self.measurementfcn(self.xk, self.T, self.wk);
149+
150+
<span class="comment">%correcting measurement</span>
151+
Xpred = self.predhistory(:, self.k);
152+
H = self.measurementj(Xpred, self.T);
153+
Ypred = self.measurementfcn(Xpred, self.T, 0);
154+
Sk = H*self.Plast*H' + self.measurementcovariance;
155+
Kk = self.Plast*H'*inv(Sk);
156+
157+
158+
159+
<span class="comment">%correct the readings</span>
160+
Xcorr = Xpred+Kk*(yk-Ypred);
161+
Pcorr = self.Plast - Kk*H*self.Plast;
162+
163+
164+
<span class="comment">%overwrite to existing values:</span>
165+
<span class="comment">% self.measurementhistory(:, self.k) = yk;</span>
166+
self.Plast = Pcorr;
167+
self.predhistory(:, self.k) = Xcorr;
168+
169+
<span class="keyword">end</span>
170+
171+
<span class="keyword">end</span>
172+
<span class="keyword">end</span>
173+
</pre><p class="footer"><br><a href="https://www.mathworks.com/products/matlab/">Published with MATLAB&reg; R2021a</a><br></p></div><!--
174+
##### SOURCE BEGIN #####
175+
classdef ExtendedKF < handle
176+
properties
177+
178+
predhistory
179+
truehistory
180+
Plast
181+
statetransitionfcn
182+
measurementfcn
183+
statej
184+
measurementj
185+
statecovariance
186+
measurementcovariance
187+
T
188+
xk
189+
k
190+
vk %plant noise
191+
wk %measurement noise
192+
state_dim
193+
measurement_dim
194+
hasadditivenoise
195+
measurementhistory
196+
end
197+
198+
methods
199+
200+
%constructor of class
201+
function self = ExtendedKF(statetransition_f, measurement_f,...
202+
state_j, measurement_j, state_covariance,...
203+
measurement_covariance, sampling_time, initial_x, additivenoise)
204+
if nargin == 9
205+
self.statetransitionfcn = statetransition_f;
206+
self.measurementfcn = measurement_f;
207+
self.statej = state_j;
208+
self.measurementj = measurement_j;
209+
self.statecovariance = state_covariance;
210+
self.measurementcovariance = measurement_covariance;
211+
self.T = sampling_time;
212+
213+
self.state_dim = size(self.statecovariance(:, 1));
214+
self.measurement_dim = size(self.measurementcovariance(:, 1));
215+
self.truehistory(:, 1) = initial_x;
216+
self.predhistory = zeros(self.state_dim);
217+
self.measurementhistory = zeros(self.measurement_dim);
218+
self.k = 1;
219+
self.hasadditivenoise = additivenoise;
220+
self.Plast = eye(self.state_dim(1));
221+
end
222+
223+
end
224+
225+
226+
function [Xpred, Ppred] = predict(self)
227+
%create plant noise
228+
self.vk = sqrt(self.statecovariance)*randn(self.state_dim(1), 1);
229+
%create noisy plant state: REFERENCE VALUE
230+
xtrue_last = self.predhistory(:, end);
231+
232+
xtrue = self.statetransitionfcn(xtrue_last, self.T, 0);
233+
self.xk = self.statetransitionfcn(xtrue_last, self.T, self.vk);
234+
235+
xhat_last = self.predhistory(:, end);
236+
F = self.statej(xhat_last, self.T);
237+
Xpred = self.statetransitionfcn(xhat_last, self.T, 0);
238+
Ppred = F*(self.Plast)*F' + self.statecovariance;
239+
240+
%save the new values:
241+
self.k = self.k+1;
242+
self.Plast = Ppred;
243+
self.predhistory(:, self.k) = Xpred;
244+
self.truehistory(:, self.k) = xtrue;
245+
246+
247+
end
248+
249+
function [Xcorr, Pcorr] = correct(self)
250+
%create measurement noise
251+
self.wk = sqrt(self.measurementcovariance)*randn(self.measurement_dim(1), 1);
252+
%create true measurement
253+
yk = self.measurementfcn(self.xk, self.T, self.wk);
254+
255+
%correcting measurement
256+
Xpred = self.predhistory(:, self.k);
257+
H = self.measurementj(Xpred, self.T);
258+
Ypred = self.measurementfcn(Xpred, self.T, 0);
259+
Sk = H*self.Plast*H' + self.measurementcovariance;
260+
Kk = self.Plast*H'*inv(Sk);
261+
262+
263+
264+
%correct the readings
265+
Xcorr = Xpred+Kk*(yk-Ypred);
266+
Pcorr = self.Plast - Kk*H*self.Plast;
267+
268+
269+
%overwrite to existing values:
270+
% self.measurementhistory(:, self.k) = yk;
271+
self.Plast = Pcorr;
272+
self.predhistory(:, self.k) = Xcorr;
273+
274+
end
275+
276+
end
277+
end
278+
##### SOURCE END #####
279+
--></body></html>

0 commit comments

Comments
 (0)