فیلتر کالمن توسعه یافته (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 بالا بیانگر شماره نمونه است. دینامیک واندرپول و سیستم اندازه‌گیری آن در توابع زیر پیاده‌سازی شده‌اند.

function  xnew = SystemDynamic(x , Ts) 
%%% x        state in sample   t
%%% xnew     state in sample   t+1
%%%  f(x)    continous dynamic of the system
%%%  Ts      Sample time of discretization


xnew = x + Ts * f(x); %% discrete using euiler method
end


function   dotX = f(x)

dotX = [x(2);(1-x(1)^2)*x(2)-x(1)];
end

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

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

function [x,P]=ekf_estimator(fstate,x,P,hmeas,z,Q,R)
%%%  FaraDars.Org    Marziey Aghaei  2020
% EKF   Extended Kalman Filter for nonlinear dynamic systems
% [x, P] = ekf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P 
%                              for nonlinear dynamic system:
%           x_k+1 = f(x_k) + w_k
%           z_k   = h(x_k) + v_k
% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
%       v ~ N(0,R) meaning v is gaussian noise with covariance R
% Inputs:   f: function handle for f(x)
%           x: "a priori" state estimate
%           P: "a priori" estimated state covariance
%           h: fanction handle for h(x)
%           z: current measurement
%           Q: process noise covariance 
%           R: measurement noise covariance
% Output:   x: "a posteriori" state estimate
%           P: "a posteriori" state covariance
%
% Example for how to use this code:
%{
n=3;      %number of state
q=0.1;    %std of process 
r=0.1;    %std of measurement
Q=q^2*eye(n); % covariance of process
R=r^2;        % covariance of measurement  
f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))];  % nonlinear state equations
h=@(x)x(1);                               % measurement equation
s=[0;0;1];                                % initial state
x=s+q*randn(3,1); %initial state          % initial state with noise
P = eye(n);                               % initial state covraiance
N=20;                                     % total dynamic steps
xV = zeros(n,N);          %estmate        % allocate memory
sV = zeros(n,N);          %actual
zV = zeros(1,N);
for k=1:N
  z = h(s) + r*randn;                     % measurments
  sV(:,k)= s;                             % save actual state
  zV(k)  = z;                             % save measurment
  [x, P] = ekf(f,x,P,h,z,Q,R);            % ekf 
  xV(:,k) = x;                            % save estimate
  s = f(s) + q*randn(3,1);                % update process 
end
for k=1:3                                 % plot results
  subplot(3,1,k)
  plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')
end
%}

[x1,A]=jaccsd(fstate,x);    %nonlinear update and linearization at current state
P=A*P*A'+Q;                 %partial update
[z1,H]=jaccsd(hmeas,x1);    %nonlinear measurement and linearization
P12=P*H';                   %cross covariance
% K=P12*inv(H*P12+R);       %Kalman filter gain
% x=x1+K*(z-z1);            %state estimate
% P=P-K*P12';               %state covariance matrix
R=chol(H*P12+R);            %Cholesky factorization
U=P12/R;                    %K=U/R'; Faster because of back substitution
x=x1+U*(R'\(z-z1));         %Back substitution to get state update
P=P-U*U';                   %Covariance update, U*U'=P12/R/R'*P12'=K*P12.

function [z,A]=jaccsd(fun,x)
% JACCSD Jacobian through complex step differentiation
% [z J] = jaccsd(f,x)
% z = f(x)
% J = f'(x)
%
z=fun(x);
n=numel(x);
m=numel(z);
A=zeros(m,n);
h=n*eps;
for k=1:n
    x1=x;
    x1(k)=x1(k)+h*i; %#ok
    A(:,k)=imag(fun(x1))/h;
end

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

clc;
clear;
close all;
%% code for state estimation of nonlinear systems using Extended Kalman Filter
%%% codes are writen based from scratch
n=2;                      %number of state
Q = diag([0.02 0.1]);     % covariance of process noise
R = 0.2;                  % covariance of measurement noise
s=[2;0];                  % initial state
x=s+Q*randn(n,1) ;        % initial state with noise
P = 1e-5*eye(n) ;         % initial state covraiance
Ts = 0.05 ;
Tf = 20;
t = 0:Ts:Tf ;
N = numel(t);
xV = zeros(n,N);          %estmate        % allocate memory
sV = zeros(n,N);          %actual
zV = zeros(3,N);
for k=1:numel(t)
    z = MeasurementDynamic(s) + R*randn(1);  % measurment with noise
    sV(:,k)= s  ;                             % save actual state
    zV(: ,k) = z;                             % save measurment
    [x, P] = ekf_estimator(@(x)SystemDynamic(x , Ts),x,P,@MeasurementDynamic , z,Q,R);            % ekf
    xV(:,k) = x;                                 % save estimate
    s = SystemDynamic(s , Ts) + Q*randn(n,1);    % update process
end
%% plot results
figure('Color',[1 1 1]);
subplot(2,2,1)
plot(t, sV(1,:), '-', t, xV(1,:), '--' , 'LineWidth' , 2); hold on
xlabel('Time  (Second)');
ylabel('First State') ;
title('Real First State Vs Its Estimation');
legend('Real','Estimated') ;
grid on
set(subplot(2,2,1),'Color',...
    [0.717647058823529 0.850980392156863 0.909803921568627],'FontSize',14);


figure(1);
subplot(2,2,2)
plot(t, sV(2,:), '-', t, xV(2,:), '--' , 'LineWidth' , 2); hold on
xlabel('Time  (Second)');
ylabel('Second State') ;
title('Real Second State Vs Its Estimation');
legend('Real','Estimated') ;
grid on
set(subplot(2,2,2),'Color',[0.623529411764706 0.83921568627451 0.929411764705882],...
    'FontSize',14);


figure(1);
subplot(2,2,3)
plot(t, sV(1,:)-xV(1,:), 'r-' , 'LineWidth' , 2); hold on
xlabel('Time  (Second)');
ylabel('First State') ;
title('Error of First State Estimation');
legend('Error 1') ;
grid on
set(subplot(2,2,3),'Color',...
    [0.866666666666667 0.929411764705882 0.780392156862745],'FontSize',14);

figure(1);
subplot(2,2,4)
plot(t, sV(2,:)-xV(2,:), 'r-' , 'LineWidth' , 2); hold on
xlabel('Time  (Second)');
ylabel('Second State') ;
title('Error of Second State Estimation');
legend('Error 2') ;
grid on
% Set the remaining axes properties
set(subplot(2,2,4),'Color',[0.905882352941176 0.96078431372549 0.835294117647059],...
    'FontSize',14);

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

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

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

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

کلیک کنید

clc;
clear;
close all;
%% code for state estimation of nonlinear systems using Extended Kalman Filter
%%% codes are writen based on matlab control toolbox 

n=2;                      %number of state
Q = diag([0.02 0.1]);     % covariance of process noise
R = 0.2;                  % covariance of measurement noise
s=[2;0];                  % initial state
x=s+Q*randn(n,1) ;        % initial state with noise
P = 1e-5*eye(n) ;         % initial state covraiance
Ts = 0.05 ;
Tf = 20;
t = 0:Ts:Tf ;
N = numel(t);
xV = zeros(n,N);          %estmate        % allocate memory
sV = zeros(n,N);          %actual
zV = zeros(3,N);

%%% type  doc extendedKalmanFilter   for more information in command window
obj = extendedKalmanFilter(@(x)SystemDynamic(x , Ts) , @MeasurementDynamic , x) ;
obj.MeasurementNoise = R ;
obj.ProcessNoise = Q ;
obj.StateCovariance = P ;

for k=1:numel(t)
    z = MeasurementDynamic(s) + R*randn(1);  % measurment with noise
    sV(:,k)= s  ;                             % save actual state
    zV(: ,k) = z;                             % save measurment
    
    predict(obj) ;
    correct(obj , z);
    x = obj.State ;
    
    xV(:,k) = x;                                 % save estimate
    s = SystemDynamic(s , Ts) + Q*randn(n,1);    % update process
end

%% plot results
figure('Color',[1 1 1]);
subplot(2,2,1)
plot(t, sV(1,:), '-', t, xV(1,:), '--' , 'LineWidth' , 2); hold on
xlabel('Time  (Second)');
ylabel('First State') ;
title('Real First State Vs Its Estimation');
legend('Real','Estimated') ;
grid on
set(subplot(2,2,1),'Color',...
    [0.717647058823529 0.850980392156863 0.909803921568627],'FontSize',14);


figure(1);
subplot(2,2,2)
plot(t, sV(2,:), '-', t, xV(2,:), '--' , 'LineWidth' , 2); hold on
xlabel('Time  (Second)');
ylabel('Second State') ;
title('Real Second State Vs Its Estimation');
legend('Real','Estimated') ;
grid on
set(subplot(2,2,2),'Color',[0.623529411764706 0.83921568627451 0.929411764705882],...
    'FontSize',14);


figure(1);
subplot(2,2,3)
plot(t, sV(1,:)-xV(1,:), 'r-' , 'LineWidth' , 2); hold on
xlabel('Time  (Second)');
ylabel('First State') ;
title('Error of First State Estimation');
legend('Error 1') ;
grid on
set(subplot(2,2,3),'Color',...
    [0.866666666666667 0.929411764705882 0.780392156862745],'FontSize',14);

figure(1);
subplot(2,2,4)
plot(t, sV(2,:)-xV(2,:), 'r-' , 'LineWidth' , 2); hold on
xlabel('Time  (Second)');
ylabel('Second State') ;
title('Error of Second State Estimation');
legend('Error 2') ;
grid on
% Set the remaining axes properties
set(subplot(2,2,4),'Color',[0.905882352941176 0.96078431372549 0.835294117647059],...
    'FontSize',14);

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

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

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

××