فیلتر کالمن توسعه یافته (EKF) در متلب — از صفر تا صد

فیلترهای کالمن به عنوان یک ابزار اساسی برای تخمین و شناسایی، در بسیاری از سیستم‌های صنعتی، رباتیک، هوافضا، خودروهای خودران و الکتریکی و... مورد استفاده قرار گرفته‌اند و هر روز بر کاربردهای آن‌ها افزوده می‌شود. در این مطلب قصد داریم با یکی دیگر از تکنیک‌های فیلتر کالمن، یعنی «فیلتر کالمن توسعه یافته» (Extended Kalman Filter) یا به اختصار EKF آشنا شویم.

فیلتر کالمن چیست؟

فیلترهای کالمن در حقیقت فیلترهای تخمین‌گر بهینه درجه دوم هستند که توسط «رودولف کالمن» اختراع شدند و هدف اساسی آن‌ها تخمین حالت‌های سیستم با داشتن اطلاعات جزیی از آن سیستم است. به عنوان مثال یک سیستم مرتبه ده را در نظر بگیرید. طبیعتا این سیستم دارای ده متغیر حالت است، اما اندازه‌گیری همه متغیرهای حالت با استفاده از ده سنسور مختلف هزینه‌های زیادی دارد و در کاربردهای عملی نمی‌توان از این روش استفاده کرد. در نتیجه فقط تعداد محدودی از متغیرهای حالت و یا تابعی از آن‌ها اندازه‌گیری می‌شود و با داشتن مدل ریاضی سیستم بقیه متغیرهای حالت باید تخمین زده شوند.

فیلم آموزش تئوری تخمین – فیلترهای کالمن یا تخمین گر بیزین در فرادرس

کلیک کنید

البته این تنها کاربرد فیلترهای کالمن نیست. گاهی اوقات ما فقط می‌توانیم یک داده‌ با نویز شدید را از سیستم دریافت کنیم. به عنوان مثالی از این حالت می‌توان به داده‌های موقعیت اندازه‌گیری‌ شده توسط سنسورهای GPS اشاره کرد. در قدم بعد می‌توان توسط یک فیلتر این داده‌ به‌ شدت نویزی را پالایش کرد و داده‌ بهتر و دقیق‌تری به‌دست آورد. یکی از بهترین فیلترهایی که در اغلب کاربردهای صنعتی برای این هدف مورد استفاده قرار می‌گیرند، فیلترهای کالمن هستند.

البته ممکن است عنوان فیلتر برای فیلتر کالمن مبهم باشد. برای رفع این ابهام یک فیلتر پایین‌ گذر را در نظر بگیرید. وظیفه فیلتر پایین‌ گذر این است که نویزهای فرکانس‌ بالا که در سیگنال ورودی وجود دارد را حذف کند تا سیگنال صاف‌تر و بهتری بدست‌ آید. به‌ صورت مشابه وظیفه فیلتر کالمن نیز این است که از داده‌های خامی که وجود دارد، اثرات نویزهای خارجی و عدم‌ قطعیت‌ها بیرون کشیده‌ شود و تخمین بهتری از اطلاعات واقعی حاصل شود. این تشابه در شکل زیر نیز به تصویر کشیده شده است. فیلتر کالمن اطلاعاتی که دارای مقداری خطا، نویز و عدم‌ قطعیت هستند را دریافت می‌کند و اطلاعات دقیق‌تری را در اختیار قرار می‌دهد. در تصویر زیر مقایسه‌ای بین فیلتر پایین گذر و فیلتر کالمن انجام شده است.

فیلتر کالمن توسعه یافته یا EKF

در مطالب قبلی مجله فرادرس به فیلتر کالمن خطی پرداخته شد. این فیلتر به منظور تخمین حالت بهینه در سیستم‌های با دینامیک خطی کاربرد دارد. اما برای سیستم‌های غیرخطی با اعمال اصلاح کوچکی می‌توان از همان فیلترهای کالمن خطی استفاده کرد. کافی است در هر مرحله از تخمین حالت‌های سیستم، از روی داده‌هایی که تخمین زده شده‌اند، دینامیک سیستم غیرخطی با استفاده از سری تیلور خطی‌سازی شود.

فیلم آموزش تئوری تخمین – فیلترهای کالمن یا تخمین گر بیزین در فرادرس

کلیک کنید

در واقع الگوریتم فیلتر کالمن خطی تا حدی تطبیقی می‌شود تا بتواند حالت‌های سیستم‌های غیرخطی را نیز تخمین بزند. باید توجه کرد که اغلب فیلترهای کالمن به‌ صورت گسسته پیاده‌سازی می‌شوند. بنابراین، دینامیک سیستم غیرخطی به‌ صورت زیر فرض می‌شود:

$$ \begin {array} {c}
\mathbf { x } _ { k } = \mathbf { f } \left( \mathbf { x } _ { k - 1 }, \mathbf { u } _ { k - 1 } \right ) + \mathbf { w } _ { k - 1 } \\
\mathbf { y } _ { k } = \mathbf { h } \left( \mathbf { x } _ { k } \right) + \mathbf { v } _ { k }
\end {array} $$

متغیرهای فرمول بالا در جدول زیر معرفی شده است.

نویز پروسه ناشی از عدم قطعیت در مدلسازی ریاضی سیستم است. واضح است که تابع f با مدل واقعی و عملی سیستم اختلاف دارد. این اختلاف در نویز پروسه مدلسازی خواهد شد. نویز اندازه‌گیری همان نویز سنسورها است که برخی از متغیرهای حالت و یا تابعی از آنها را اندازه‌گیری می‌کند. نویز اندازه‌گیری و پروسه اغلب به صورت فرآیند‌های گاوسی مدلسازی می‌شوند و ماتریس کواریانس آن‌ها را نیز به ترتیب با $$ R _ k $$ و $$ Q _ k $$ مشخص می‌کنند. با انجام تحلیل‌های آماری بر روی سنسورها می‌توان ماتریس کواریانس اندازه‌گیری را مشخص کرد ولی به‌دلیل تنوع و پیچیدگی سیستم‌های غیرخطی، الگوریتم مشخصی برای تعیین ماتریس کواریانس نویز پروسه وجود ندارد و در عمل اغلب به‌ صورت سعی و خطایی تنظیم می‌شود.

الگوریتم فیلتر کالمن توسعه یافته در دو فاز انجام خواهد شد. فرض کنید تمام اطلاعات سیستم تا نمونه k-1 در دسترس باشد. در این‌ صورت بر اساس مدل ریاضی سیستم و سیگنال‌های موجود تا زمان k-1، یک تخمین اولیه از متغیرهای حالت در نمونه k محاسبه می‌شود. این گام از فیلتر کالمن را فرآیند پیش‌بینی می‌گوییم. حال طبیعتا در زمان k یک اندازه‌گیری جدید توسط سنسورها انجام می‌شود یعنی $$ y _ k $$ در دسترس قرار می‌گیرد. در گام دوم فیلتر کالمن که فاز به‌روزرسانی نام دارد، پیش‌بینی انجام‌ شده در گام اول بر اساس داده‌ی جدیدی که در دسترس قرار گرفته است، بهبود داده می‌شود تا حالت‌های تخمینی نهایی در نمونه kام بدست آید. فاز پیش‌بینی توسط دو معادله زیر انجام خواهد شد:

$$ \begin {aligned}
& \hat { \mathbf { x } } _ { k | k - 1 } = \mathbf { f } \left ( \hat { \mathbf { x } } _ { k - 1 }, \mathbf { u } _ { k - 1 } \right)\\
& \mathbf { P } _ { k | k - 1 } = \mathbf { F } _ { k - 1 } \mathbf { P } _ { k - 1 } \mathbf { F } _ { k - 1 } ^ { T } + \mathbf { Q } _ { k - 1 }
\end {aligned} $$

در روابط بالا $$ \hat { \mathbf { x } } _ { k - 1 } $$ بردار حالت‌های تخمینی در نمونه k-1 و $$ \mathbf { P } _ { k - 1 } $$ ماتریس کواریانس در نمونه k-1 و $$ \hat { \mathbf { x } } _ { k | k - 1 } $$ پیش‌بینی متغیر حالت در نمونه k بر اساس اطلاعات سیستم تا نمونه k-1 و $$ \mathbf { P } _ { k | k - 1 } $$ نیز پیش‌بینی ماتریس کواریانس در نمونه k بر اساس اطلاعات سیستم تا نمونه k-1 است. ماتریس‌ $$ \mathbf { F } _ { k  } $$ دینامیک حالت خطی‌ شده است که از طریق خطی‌سازی تیلور به‌ صورت زیر به‌دست می‌آید:

$$ \mathbf { F } _ { k } = \left. \frac { \partial \mathbf { f } } { \partial \mathbf { x } } \right | _ { \mathbf { x } _ { k - 1 } } $$

$$ \mathbf { H } _ { k } $$ نیز دینامیک اندازه‌گیری خطی‌ شده است که در فاز به‌روزرسانی استفاده می‌شود. این ماتریس نیز به صورت  زیر محاسبه می‌شود:

$$ \mathbf { H } _ { k } = \left . \frac { \partial \mathbf { h } } { \partial \mathbf { x } } \right | _ { \hat { \mathbf { x } } _ { k | k - 1 } } $$

فاز به‌روزرسانی فیلتر کالمن توسعه یافته با استفاده از فرمول‌های زیر انجام خواهد شد:

$$ \begin {aligned}
& \mathbf { K } _ { k } = \mathbf { P } _ { k | k - 1 } \mathbf { H } _ { k } ^ { T } \left ( \mathbf { H } _ { k } \mathbf { P } _ {k | k - 1 } \mathbf { H } _ { k } ^ { T } + \mathbf { R } _ { k } \right) ^ { - 1 } \\
& \hat { \mathbf { x } } _ { k } = \hat { \mathbf { x } } _ { k | k - 1 } + \mathbf { K } _ { k } \left[ \mathbf { z } _ { k } - \mathbf { h } \left( \hat { \mathbf { x } } _ { k | k - 1 } \right) \right] \\
& \mathbf { P } _ { k } = \left( \mathbf { I } - \mathbf { K } _ { k } \mathbf { H } _ { k } \right) \mathbf { P } _ { k | k - 1 }
\end {aligned} $$

در فرمول بالا $$ \mathbf { z } _ { k }  $$ همان خروجی اندازه‌گیری‌ شده توسط سنسورها است. بنابراین، در قدم اول بهره کالمن ($$ \mathbf { K } _ { k } $$) محاسبه می‌شود و سپس تخمین نهایی متغیرهای حالت ($$ \hat { \mathbf { x } } _ { k } $$) و ماتریس کواریانس ($$ \mathbf { P } _ { k } $$) در نمونه k محاسبه خواهد شد. اگر به عبارت به‌روزرسانی حالت‌ها دقت کنیم، متوجه خواهیم شد که در فاز به‌روزرسانی به حالت پیش‌بینی‌ شده در گام اول یک ترم جدید اضافه شده است که متناسب با خطای پیش‌بینی است. عبارت $$ \mathbf { z } _ { k } - \mathbf { h } ( \hat { \mathbf { x } } _ { k | k - 1 } ) $$ دقیقا بیانگر اختلاف بین خروجی واقعی اندازه‌گیری‌شده توسط سنسورها و خروجی به‌دست آمده در فاز پیش‌بینی است. در دیاگرام تصویر زیر دو فاز پیش‌بینی و به‌روزرسانی در فیلتر کالمن توسعه یافته یا EKF نشان داده شده است.

اگر دینامیک سیستم به‌ شدت غیرخطی نباشد، در این‌ صورت با استفاده از فیلتر کالمن توسعه یافته می‌توان با دقت بسیار خوبی حالت‌های سیستم را تخمین زد. اما اگر سیستم به‌ شدت غیرخطی باشد، در این صورت بهتر است از فیلتر کالمن بی‌اثر استفاده شود که در مطالب قبلی مجله فرادرس مورد بررسی قرار گرفته است.

پیاده‌سازی فیلتر کالمن توسعه یافته یا EKF در متلب

در ادامه کدنویسی تخمین حالت‌ سیستم‌های غیرخطی توسط الگوریتم فیلتر کالمن توسعه یافته به دو شکل در نرم‌افزار MATLAB ارائه می‌شود. در کد اول با استفاده از دستورات پایه متلب و خط به خط برنامه‌نویسی انجام شده است و عملکرد الگوریتم برای تخمین حالت‌های دینامیک غیرخطی واندرپول نیز مورد بررسی قرار گرفته است. در برنامه دوم با استفاده از تولباکس‌های آماده نرم افزار متلب کدنویسی انجام شده است.

فیلم آموزش جعبه ابزار شناسایی سیستم در متلب در فرادرس

کلیک کنید

برنامه اول برای افرادی مناسب است که حوزه تحقیقاتی آنها در مورد خود الگوریتم فیلتر کالمن توسعه یافته است و برنامه دوم نیز برای محققینی قابل استفاده است که صرفا یک الگوریتم برای تخمین حالت سیستم‌های غیرخطی لازم دارند. معادلات دینامیکی سیستم غیرخطی واندرپول به صورت زیر تعریف می‌شوند. رفتار سیستم نوسانی و آشوبناک است و فرض می‌کنیم موقعیت اولیه ۲ و سرعت اولیه حالت نیز ۰ باشد.

$$ \ddot { y } ( t ) + \left ( 1 - y ^ { 2 } ( t ) \right) \dot { y } ( t ) + y ( t ) = 0 $$

در قدم اول باید سیستم را به فرم فضای حالت نوشت. متغیرهای حالت موقعیت و سرعت اولیه انتخاب می‌شوند. در نتیجه بیان فضای حالت سیستم و خروجی قابل‌ اندازه‌گیری به صورت زیر خواهد بود:

$$ \begin {aligned}
&\dot { x } ( t ) = \left [ \begin {array} { l }
x _ { 2 } ( t ) \\
\left ( 1 - x _ { 1 } ^ { 2 } ( t ) \right ) x _ { 2 } ( t ) + x _ { 1 } ( t )
\end {array} \right] = f ( x ( t ) ) \\
& y = x _ { 1 } ( t ) = h ( x ( t ) )
\end {aligned} $$

در قدم بعد باید توجه کرد که الگوریتم فیلتر کالمن توسعه یافته برای تخمین حالت سیستم‌های گسسته استفاده می‌شود. بنابراین با استفاده از تقریب اویلر برای مشتق می‌توان سیستم را به شکل زیر گسسته‌سازی کرد:

$$ x ( t + 1 ) = x ( t ) + T _ { s } f ( x ( t ) ) $$

$$ T _ { s } $$ زمان نمونه‌برداری است و مقدار آن در شبیه‌سازی برابر با ۰٫۰۵ انتخاب می‌شود. طول بازه شبیه‌سازی نیز ۲۰ ثانیه در نظر گرفته می‌شود. همچنین باید توجه کرد که در رابطه t بالا بیانگر شماره نمونه است. دینامیک واندرپول و سیستم اندازه‌گیری آن در توابع زیر پیاده‌سازی شده‌اند.

1function  xnew = SystemDynamic(x , Ts) 
2%%% x        state in sample   t
3%%% xnew     state in sample   t+1
4%%%  f(x)    continous dynamic of the system
5%%%  Ts      Sample time of discretization
6
7
8xnew = x + Ts * f(x); %% discrete using euiler method
9end
10
11
12function   dotX = f(x)
13
14dotX = [x(2);(1-x(1)^2)*x(2)-x(1)];
15end

1function  y = MeasurementDynamic(x)
2y = x(1);
3end

الگوریتم تخمین حالت برای سیستم‌های غیرخطی توسط EKF نیز در تابع متلب زیر پیاده‌سازی شده است.

1function [x,P]=ekf_estimator(fstate,x,P,hmeas,z,Q,R)
2%%%  FaraDars.Org    Marziey Aghaei  2020
3% EKF   Extended Kalman Filter for nonlinear dynamic systems
4% [x, P] = ekf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P 
5%                              for nonlinear dynamic system:
6%           x_k+1 = f(x_k) + w_k
7%           z_k   = h(x_k) + v_k
8% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
9%       v ~ N(0,R) meaning v is gaussian noise with covariance R
10% Inputs:   f: function handle for f(x)
11%           x: "a priori" state estimate
12%           P: "a priori" estimated state covariance
13%           h: fanction handle for h(x)
14%           z: current measurement
15%           Q: process noise covariance 
16%           R: measurement noise covariance
17% Output:   x: "a posteriori" state estimate
18%           P: "a posteriori" state covariance
19%
20% Example for how to use this code:
21%{
22n=3;      %number of state
23q=0.1;    %std of process 
24r=0.1;    %std of measurement
25Q=q^2*eye(n); % covariance of process
26R=r^2;        % covariance of measurement  
27f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))];  % nonlinear state equations
28h=@(x)x(1);                               % measurement equation
29s=[0;0;1];                                % initial state
30x=s+q*randn(3,1); %initial state          % initial state with noise
31P = eye(n);                               % initial state covraiance
32N=20;                                     % total dynamic steps
33xV = zeros(n,N);          %estmate        % allocate memory
34sV = zeros(n,N);          %actual
35zV = zeros(1,N);
36for k=1:N
37  z = h(s) + r*randn;                     % measurments
38  sV(:,k)= s;                             % save actual state
39  zV(k)  = z;                             % save measurment
40  [x, P] = ekf(f,x,P,h,z,Q,R);            % ekf 
41  xV(:,k) = x;                            % save estimate
42  s = f(s) + q*randn(3,1);                % update process 
43end
44for k=1:3                                 % plot results
45  subplot(3,1,k)
46  plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')
47end
48%}
49
50[x1,A]=jaccsd(fstate,x);    %nonlinear update and linearization at current state
51P=A*P*A'+Q;                 %partial update
52[z1,H]=jaccsd(hmeas,x1);    %nonlinear measurement and linearization
53P12=P*H';                   %cross covariance
54% K=P12*inv(H*P12+R);       %Kalman filter gain
55% x=x1+K*(z-z1);            %state estimate
56% P=P-K*P12';               %state covariance matrix
57R=chol(H*P12+R);            %Cholesky factorization
58U=P12/R;                    %K=U/R'; Faster because of back substitution
59x=x1+U*(R'\(z-z1));         %Back substitution to get state update
60P=P-U*U';                   %Covariance update, U*U'=P12/R/R'*P12'=K*P12.
61
62function [z,A]=jaccsd(fun,x)
63% JACCSD Jacobian through complex step differentiation
64% [z J] = jaccsd(f,x)
65% z = f(x)
66% J = f'(x)
67%
68z=fun(x);
69n=numel(x);
70m=numel(z);
71A=zeros(m,n);
72h=n*eps;
73for k=1:n
74    x1=x;
75    x1(k)=x1(k)+h*i; %#ok
76    A(:,k)=imag(fun(x1))/h;
77end

سپس در برنامه زیر تخمین حالت‌ سیستم غیرخطی تعریف‌ شده انجام می‌گیرد.

1clc;
2clear;
3close all;
4%% code for state estimation of nonlinear systems using Extended Kalman Filter
5%%% codes are writen based from scratch
6n=2;                      %number of state
7Q = diag([0.02 0.1]);     % covariance of process noise
8R = 0.2;                  % covariance of measurement noise
9s=[2;0];                  % initial state
10x=s+Q*randn(n,1) ;        % initial state with noise
11P = 1e-5*eye(n) ;         % initial state covraiance
12Ts = 0.05 ;
13Tf = 20;
14t = 0:Ts:Tf ;
15N = numel(t);
16xV = zeros(n,N);          %estmate        % allocate memory
17sV = zeros(n,N);          %actual
18zV = zeros(3,N);
19for k=1:numel(t)
20    z = MeasurementDynamic(s) + R*randn(1);  % measurment with noise
21    sV(:,k)= s  ;                             % save actual state
22    zV(: ,k) = z;                             % save measurment
23    [x, P] = ekf_estimator(@(x)SystemDynamic(x , Ts),x,P,@MeasurementDynamic , z,Q,R);            % ekf
24    xV(:,k) = x;                                 % save estimate
25    s = SystemDynamic(s , Ts) + Q*randn(n,1);    % update process
26end
27%% plot results
28figure('Color',[1 1 1]);
29subplot(2,2,1)
30plot(t, sV(1,:), '-', t, xV(1,:), '--' , 'LineWidth' , 2); hold on
31xlabel('Time  (Second)');
32ylabel('First State') ;
33title('Real First State Vs Its Estimation');
34legend('Real','Estimated') ;
35grid on
36set(subplot(2,2,1),'Color',...
37    [0.717647058823529 0.850980392156863 0.909803921568627],'FontSize',14);
38
39
40figure(1);
41subplot(2,2,2)
42plot(t, sV(2,:), '-', t, xV(2,:), '--' , 'LineWidth' , 2); hold on
43xlabel('Time  (Second)');
44ylabel('Second State') ;
45title('Real Second State Vs Its Estimation');
46legend('Real','Estimated') ;
47grid on
48set(subplot(2,2,2),'Color',[0.623529411764706 0.83921568627451 0.929411764705882],...
49    'FontSize',14);
50
51
52figure(1);
53subplot(2,2,3)
54plot(t, sV(1,:)-xV(1,:), 'r-' , 'LineWidth' , 2); hold on
55xlabel('Time  (Second)');
56ylabel('First State') ;
57title('Error of First State Estimation');
58legend('Error 1') ;
59grid on
60set(subplot(2,2,3),'Color',...
61    [0.866666666666667 0.929411764705882 0.780392156862745],'FontSize',14);
62
63figure(1);
64subplot(2,2,4)
65plot(t, sV(2,:)-xV(2,:), 'r-' , 'LineWidth' , 2); hold on
66xlabel('Time  (Second)');
67ylabel('Second State') ;
68title('Error of Second State Estimation');
69legend('Error 2') ;
70grid on
71% Set the remaining axes properties
72set(subplot(2,2,4),'Color',[0.905882352941176 0.96078431372549 0.835294117647059],...
73    'FontSize',14);

با اجرای برنامه بالا، خروجی به صورت شکل‌های زیر به دست می‌آید. در گراف‌های بالایی نمودارهای آبی‌ رنگ حالت‌های واقعی سیستم هستند و نمودارهای قرمز‌ رنگ نیز تخمین آن‌ها توسط الگوریتم EKF است. گراف‌های پایینی نیز خطای بین حالت‌های واقعی سیستم و مقادیر تخمینی آن‌ها را نشان می‌دهند. کاملا واضح است که عملکرد تخمین‌گر رضایت‌بخش‌ است و به خوبی توانسته است حالت‌های سیستم غیرخطی واندرپول را تخمین بزند. همچنین باید توجه کرد که در برنامه برای اینکه عملکرد الگوریتم تخمینگر با وضوح بیشتری نشان داده شود، نویز پروسه و نویز اندازه‌گیری مقادیر بالایی درنظر گرفته شده است.

پیاده‌سازی فیلتر کالمن توسعه یافته با استفاده از دستور extendedKalmanFilter متلب

در حالت بعد الگوریتم EKF با استفاده از توابع آماده متلب پیاده‌سازی خواهد شد. با استفاده از دستور extendedKalmanFilter فیلتر کالمن توسعه یافته ایجاد خواهد شد. با دستور predict فاز پیش‌بینی و با دستور correct نیز فاز به‌روزرسانی‌ حالت‌های تخمین و ماتریس کواریانس انجام خواهد شد. به منظور تخمین حالت توسط این توابع ابتدا همانند برنامه اول توابع دینامیک سیستم و اندازه‌گیری تعریف خواهد شد و سپس از برنامه زیر استفاده می‌کنیم.

فیلم آموزش جعبه ابزار شناسایی سیستم در متلب در فرادرس

کلیک کنید

1clc;
2clear;
3close all;
4%% code for state estimation of nonlinear systems using Extended Kalman Filter
5%%% codes are writen based on matlab control toolbox 
6
7n=2;                      %number of state
8Q = diag([0.02 0.1]);     % covariance of process noise
9R = 0.2;                  % covariance of measurement noise
10s=[2;0];                  % initial state
11x=s+Q*randn(n,1) ;        % initial state with noise
12P = 1e-5*eye(n) ;         % initial state covraiance
13Ts = 0.05 ;
14Tf = 20;
15t = 0:Ts:Tf ;
16N = numel(t);
17xV = zeros(n,N);          %estmate        % allocate memory
18sV = zeros(n,N);          %actual
19zV = zeros(3,N);
20
21%%% type  doc extendedKalmanFilter   for more information in command window
22obj = extendedKalmanFilter(@(x)SystemDynamic(x , Ts) , @MeasurementDynamic , x) ;
23obj.MeasurementNoise = R ;
24obj.ProcessNoise = Q ;
25obj.StateCovariance = P ;
26
27for k=1:numel(t)
28    z = MeasurementDynamic(s) + R*randn(1);  % measurment with noise
29    sV(:,k)= s  ;                             % save actual state
30    zV(: ,k) = z;                             % save measurment
31    
32    predict(obj) ;
33    correct(obj , z);
34    x = obj.State ;
35    
36    xV(:,k) = x;                                 % save estimate
37    s = SystemDynamic(s , Ts) + Q*randn(n,1);    % update process
38end
39
40%% plot results
41figure('Color',[1 1 1]);
42subplot(2,2,1)
43plot(t, sV(1,:), '-', t, xV(1,:), '--' , 'LineWidth' , 2); hold on
44xlabel('Time  (Second)');
45ylabel('First State') ;
46title('Real First State Vs Its Estimation');
47legend('Real','Estimated') ;
48grid on
49set(subplot(2,2,1),'Color',...
50    [0.717647058823529 0.850980392156863 0.909803921568627],'FontSize',14);
51
52
53figure(1);
54subplot(2,2,2)
55plot(t, sV(2,:), '-', t, xV(2,:), '--' , 'LineWidth' , 2); hold on
56xlabel('Time  (Second)');
57ylabel('Second State') ;
58title('Real Second State Vs Its Estimation');
59legend('Real','Estimated') ;
60grid on
61set(subplot(2,2,2),'Color',[0.623529411764706 0.83921568627451 0.929411764705882],...
62    'FontSize',14);
63
64
65figure(1);
66subplot(2,2,3)
67plot(t, sV(1,:)-xV(1,:), 'r-' , 'LineWidth' , 2); hold on
68xlabel('Time  (Second)');
69ylabel('First State') ;
70title('Error of First State Estimation');
71legend('Error 1') ;
72grid on
73set(subplot(2,2,3),'Color',...
74    [0.866666666666667 0.929411764705882 0.780392156862745],'FontSize',14);
75
76figure(1);
77subplot(2,2,4)
78plot(t, sV(2,:)-xV(2,:), 'r-' , 'LineWidth' , 2); hold on
79xlabel('Time  (Second)');
80ylabel('Second State') ;
81title('Error of Second State Estimation');
82legend('Error 2') ;
83grid on
84% Set the remaining axes properties
85set(subplot(2,2,4),'Color',[0.905882352941176 0.96078431372549 0.835294117647059],...
86    'FontSize',14);

با اجرای برنامه بالا خروجی به صورت زیر به دست می‌آید. نتایج مشابه برنامه اول است و نشانگر عملکرد فوق‌العاده الگوریتم تخمین حالت غیرخطی EKF برای سیستم‌های دینامیکی غیرخطی است.

اگر این مطلب برای شما مفید بوده است، آموزش‌های زیر نیز به شما پیشنهاد می‌شوند:

• مجموعه آموزش‌های دروس مهندسی کنترل
• آموزش کنترل مدرن به همراه پیاده سازی در متلب​‎
• مجموعه آموزش‌های مهندسی برق
• آموزش کاربرد متلب در مهندسی کنترل
• کنترل بهینه در متلب — از صفر تا صد
• کنترل پیش بین مدل (MPC) — راهنمای جامع
• معیار پایداری نایکویست در سیستم کنترل — از صفر تا صد

××