فیلتر کالمن خنثی
برای درس کنترل فرایندهای تصادفی رشته برق در مقطع ارشد نیاز به شبیه سازی یک مقاله کاملا مرتبط با این درس دارم که شبیه سازی با متلب شده باشه و یک گزارش کار کوچک هم داشته باشه.
گزارش پروژه فیلتر کالمن با متلب
مقدمه
در بسیاری از زمینه های علمی، از مدلهای یقینی برای توصیف دینامیک های یک سیستم استفاده میشود مانند ربات، پاندول و امثال اینها. واژه دینامیک بدین معناست که ما بر اصول و قوانین حاکم بر سیستم موردنظر مسلط بوده و می دانیم که سیستم چگونه تکامل می یابد. برای مثال در یک ربات سیار می خواهیم موقعیت آینده ربات را پیشبینی کرده یا تخمین بزنیم و اطلاعاتی را بدست آوریم که این اطلاعات را می توان با استفاده از موقعیت فعلی ربات و تمام فرامین های ارسالی به ربات، استخراج کرد. این همان مدل دینامیکی ربات است. از طرفی، در دنیای پیرامون ما همواره نویز (فرایند) وجود دارد. این نویز باعث میشود دقت مدل کنترل دینامیکی کاهش یابد. در نتیجه ما همواره اطلاعاتی را درباره موقعیت ربات به دلیل وجود نویز از دست میدهیم. در کل، میتوان از سنسورهای موقعیت برای بدست آوردن اطلاعاتی درباره موقعیت فعلی ربات استفاده کرد، اما مقادیر اندازه گیری شده توسط این سنسورها نیز به دلیل وجود نویز اندازه گیری غیرقابل اعتماد است. در نتیجه به دلیل وجود نویزهای مختلف نمیتوان ربات را به خوبی موقعیت یابی کرد. فیلتر کالمن یک ابزار قدرتمند برای حذف اینگونه نویزهاست و یک تخمین دقیقی از موقعیت ربات را به ما میدهد. البته این یکی از کاربردهای فیلتر کالمن است، در صورتی این فیلتر کاربردهای بسیاری را در اکثر زمینه های علمی به ویژه مهندسی برق از خود نشان داده است. اگر سیستم مورد مطالعه خطی باشد میتوان از فیلتر کالمن (معمولی) استفاده کرد، اما در بسیاری از کاربردها، دینامیک های سیستم خطی نیستند لذا نمیتوان از فیلتر کالمن (معمولی) برای تخمین حالتهای چنین سیستمی استفاده کرد. در اینگونه مواقع از تعمیم های این فیلتر استفاده میشود. یکی از تعمیم های آن، فیلتر کالمن خنثی است که در ادامه به شرح آن میپردازیم.
فیلتر کالمن خنثی
مقدمه
فیلتر کالمن خنثی (UKF) یک ابزار مهندسی قدرتمند جهت تخمین دینامیک های یک سیستم غیرخطی در بسیاری از کاربردهای عملی است. این فیلتر برای مقاصد زیادی به کار گرفته میشود از جمله تخمین باد، تخمین سرعت هوا و امثال اینها. به دلیل عدم نیاز به محاسبه ماتریس ژاکوبین، فیلتر کالمن خنثی نسبت به سایر انواع فیلتر کالمن از جمله فیلتر کالمن توسعه یافته، در مراحل طراحی راحتتر مورد استفاده قرار می گیرد.
فیلتر کالمن خنثی
UKF به عنوان فیلتری منتج از فیلتر کالمن معمولی معرفی شده است. در روش UKF مبنا بر دو اصل اساسی ذیل بنا نهاده شده است. اولا، به آسانی میتوان یک تبدیل غیرخطی را بر یک نقطه تنها، به جای کل تابع چگالی احتمال (PDF) اجرا نمود. ثانیا، چندان مشکل نیست که بتوان مجموعه نقاط منحصر به فردی در فضای حالت پیدا نمود که PDF این مجموعه، تقریب خوبی از PDF واقعی بردار حالت باشد. یک سیستم دینامیکی غیرخطی را میتوان با نمایش فضای حالت گسسته در زمان به صورت زیر فرض نمود:
که در آن xk بردار متغیرهای حالت سیستم، uk بردار ورودیهای خارجی معلوم، yk بردار سیگنال اندازه گیری مشاهده شده است. همچنین vk و nk به ترتیب معرف نویز فرایند و نویز اندازه گیری هستند. روش UKF با به کارگیری مجموعه نقاط Sigma Points که تبدیل خنثی (UT) نامیده میشود، برای انتشار میانگین و کوواریانس متغیرهای حالت سیستم به صورت بازگشتی و تحت شرایط گوسی بودن نویزها بهره میگیرد. درتبدیل خنثی که فیلتر کالمن بر اساس آن بنا شده است، یک مجموعه وزندار از نقاط سیگما به طور قطعی به گونهای انتخاب میشود که خصوصیت معلوم این نقاط با توزیع اولیه مطابقت داشته باشد. سپس هر نقطه از تبدیل غیرخطی عبور داده میشود و خصوصیت مجموعه تبدیل یافته محاسبه میشود. با این مجموعه نقاط، تبدیل خنثی کارایی یکسانی شبیه فیلتر گوسی مرتبه دوم کوتاه شده، با مرتبه محاسبات یکسان با فیلتر کالمن توسعه یافته خواهد داشت، اما نیازی به محاسبه ماتریس ژاکوبین یا ماتریس هسین ندارد.
تعمیم روش UKF در تخمین پارامترهای سیستم دینامیکی غیرخطی گاهی اوقات شناسایی سیستم نیز نامیده میشود. حال نمایش فضای حالت زیر را در نظر بگیرید:
که در آن uk بردار ورودی، yk بردار خروجی وتابع غیرخطی h(.) توسط بردار xk پارامتری میشود. همچنین qk و rk معرف نویز سفید گاوسی با میانگین صفر و به ترتیب ماتریس کوواریانس Q و R هستند.
در مساله تخمین پارامتر هدف یافتن بردار پارامترهای xk از روی دادههای اندازهگیری خروجی، در حضور نویز و سایر عدم قطعیت ها است. مراحل محاسباتی فیلتر UKF در تخمین پارامترهای سیستم معرفی شده با نمایش فضای حالت به صورت معادلات فوق به فرم خلاصه به صورت الگوریتم زیر است:
که در آن n تعداد حالتها (پارامترهای مورد تخمین) بوده، κ و α و β و پارامترهای مقیاسکننده نامیده میشوند و انتخاب مناسب برای آنها =0κ و e-4 £ a £ 1 و b = 2 میباشد.
مثالی از کاربرد فیلتر کالمن خنثی
در این بخش با ارائهی مثالی به تفصیل فیلتر کالمن خنثی میپردازیم. مساله ردیابی یک شی متحرک در یک فضای دوبعدی را در نظر بگیرید (یکی از بعدها را شمال N و دیگری را شرق E فرض میکنیم). شتاب این شی در هر دو بعد از نویزهای سفید مستقل تشکیل شده است. دو موقعیت ردیابی، در مکانهای (N1, El)=(20 , 0) و (N2, E2)=(0 , 20) از صفحهی مختصات دوبعدی مفروض است. مدل فضای حالت سیستم به صورت زیر است:
که در آن nk و ek به ترتیب مختصات شمالی و شرقی شی در لحظه k و= 0.1 T زمان نمونه برداری سیستم است. همچنین wk و vk به ترتیب نویزهای فرآیند و اندازهگیری با میانگین صفر و ماتریسهای کوواریانس Q = diag(0, 0, 4, 4) و R = diag(1, 1) هستند. شرایط اولیه سیستم نیز به صورت زیر است:
0=[0 0 50 50]T , P0=I
که در آن I ماتریس واحد است. با توجه به مدل فضای حالت سیستم، معادلات حالت سیستم، خطی و معادلات خروجی آن غیرخطی هستند. بنابراین در کل با یک سیستم غیرخطی روبهرو هستیم. بنابراین برای تخمین موقعیت شی (متغیرهای حالت) در صفحه مختصات دوبعدی باید از فیلترهای غیرخطی کالمن مانند فیلتر کالمن توسعه یافته (EKF) و یا فیلتر کالمن خنثی (UKF) استفاده کنیم. در اینجا هر دو نوع فیلتر را به کار برده و نتایج حاصل را با مقایسه میکنیم. قطعه کد متلب مربوط به این مثال به صورت زیر است:
%% Before filter execution
% System properties
T = 0.1; % Sampling time
N = 600; % Number of time steps for filter
N1 = 20; % Station 1 North coordinate
E1 = 0; % Station 1 East coordinate
N2 = 0; % Station 2 North coordinate
E2 = 20; % Station 2 East coordinate
% Step 1: Define UT Scaling parameters and weight vectors
L = 4; % Size of state vector
alpha = 1; % Primary scaling parameter
beta = 2; % Secondary scaling parameter (Gaussian assumption)
kappa = 0; % Tertiary scaling parameter
lambda = alpha^2*(L+kappa) - L;
wm = ones(2*L + 1,1)*1/(2*(L+lambda));
wc = wm;
wm(1) = lambda/(lambda+L);
wc(1) = lambda/(lambda+L) + 1 - alpha^2 + beta;
% Step 2: Define noise assumptions
Q = diag([0 0 4 4]);
R = diag([1 1]);
% Step 3: Initialize state and covariance
x = zeros(4, N); % Initialize size of state estimate for all k
x(:,1) = [0; 0; 50; 50]; % Set initial state estimate
P0 = eye(4,4); % Set initial error covariance
% Simulation Only: Calculate true state trajectory for comparison
% Also calculate measurement vector
w = sqrt(Q)*randn(4, N); % Generate random process noise (from assumed Q)
v = sqrt(R)*randn(2, N); % Generate random measurement noise (from assumed R)
xt = zeros(4, N); % Initialize size of true state for all k
xt(:,1) = [0; 0; 50; 50] + sqrt(P0)*randn(4,1); % Set true initial state
yt = zeros(2, N); % Initialize size of output vector for all k
for k = 2:N
xt(:,k) = [1 0 T 0; 0 1 0 T; 0 0 1 0; 0 0 0 1]*xt(:,k-1) + w(:,k-1);
yt(:,k) = [sqrt((xt(1,k)-N1)^2 + (xt(2,k)-E1)^2); ...
sqrt((xt(1,k)-N2)^2 + (xt(2,k)-E2)^2)] + v(:,k);
end
%% Initialize and run EKF for comparison
xe = zeros(4,N);
xe(:,1) = x(:,1);
P = P0;
F = [1 0 T 0; 0 1 0 T; 0 0 1 0; 0 0 0 1]; % Linear prediction
for k = 2:N
% Prediction
x_m = F*xe(:,k-1);
P_m = F*P*F' + Q;
% Observation
y_m = [sqrt((x_m(1)-N1).^2 + (x_m(2)-E1).^2); ...
sqrt((x_m(1)-N2).^2 + (x_m(2)-E2).^2)];
H = [(x_m(1)-N1)/sqrt((x_m(1)-N1)^2 + (x_m(2)-E1)^2), ...
(x_m(2)-E1)/sqrt((x_m(1)-N1)^2 + (x_m(2)-E1)^2), 0, 0; ...
(x_m(1)-N2)/sqrt((x_m(1)-N2)^2 + (x_m(2)-E2)^2), ...
(x_m(2)-E2)/sqrt((x_m(1)-N2)^2 + (x_m(2)-E2)^2), 0, 0];
% Measurement Update
K = P_m*H'/(H*P_m*H' + R); % Calculate Kalman gain
xe(:,k) = x_m + K*(yt(:,k) - y_m); % Update state estimate
P = (eye(4)-K*H)*P_m; % Update covariance estimate
end
%% Execute Unscented Kalman Filter
P = P0; % Set first value of P to the initial P0
for k = 2:N
% Step 1: Generate the sigma-points
sP = chol(P,'lower'); % Calculate square root of error covariance
% chi_p = "chi previous" = chi(k-1)
chi_p = [x(:,k-1), x(:,k-1)*ones(1,L)+sqrt(L+lambda)*sP, ...
x(:,k-1)*ones(1,L)-sqrt(L+lambda)*sP];
% Step 2: Prediction Transformation
% Propagate each sigma-point through prediction
% chi_m = "chi minus" = chi(k|k-1)
chi_m = [1 0 T 0; 0 1 0 T; 0 0 1 0; 0 0 0 1]*chi_p;
x_m = chi_m*wm; % Calculate mean of predicted state
% Calculate covariance of predicted state
P_m = Q;
for i = 1:2*L+1
P_m = P_m + wc(i)*(chi_m(:,i) - x_m)*(chi_m(:,i) - x_m)';
end
% Step 3: Observation Transformation
% Propagate each sigma-point through observation
psi_m = [sqrt((chi_m(1,:)-N1).^2 + (chi_m(2,:)-E1).^2); ...
sqrt((chi_m(1,:)-N2).^2 + (chi_m(2,:)-E2).^2)];
y_m = psi_m*wm; % Calculate mean of predicted output
% Calculate covariance of predicted output
% and cross-covariance between state and output
Pyy = R;
Pxy = zeros(L,2);
for i = 1:2*L+1
Pyy = Pyy + wc(i)*(psi_m(:,i) - y_m)*(psi_m(:,i) - y_m)';
Pxy = Pxy + wc(i)*(chi_m(:,i) - x_m)*(psi_m(:,i) - y_m)';
end
% Step 4: Measurement Update
K = Pxy/Pyy; % Calculate Kalman gain
x(:,k) = x_m + K*(yt(:,k) - y_m); % Update state estimate
P = P_m - K*Pyy*K'; % Update covariance estimate
end
%% Display results
figure(1);
t = T*(1:N);
for i = 1:4
subplot(4,2,2*i-1); plot(t,x(i,:),'b-', t,xe(i,:),'g-.', t,xt(i,:),'r--', 'LineWidth', 2);
xlabel('Time (s)'); ylabel(['x_',num2str(i)]); grid on; legend('UKF','EKF','True');
subplot(4,2,2*i); plot(t,x(i,:)-xt(i,:),'b-', t,xe(i,:)-xt(i,:),'g-.', 'LineWidth', 2);
xlabel('Time (s)'); ylabel(['\Deltax_',num2str(i)]); grid on; legend('UKF','EKF');
end
نتایج شبیه سازی به صورت نمودارهای زیر بدست آمده است:
شکل - مقایسه فیلترهای EKF و UKF در تخمین حالت های یک سیستم غیرخطی
نتیجه گیری
فیلترهای EKF و UKF هر کدام برای مقاصد خاصی به کار برده میشود. در واقع، در کاربردهای زیادی میتوان هر دو فیلتر را به طور موثر استفاده کرد و از تفاوت موجود بین آنها چشمپوشی کرد. اما در سیستمهای با میزان غیرخطی بالا، استفاده از UKF ارجحیت دارد. یکی دیگر از مزایای UKF این است که از نظر پیاده سازی نسبت به EKF راحتتر است که این ویژگی آن را به یک ابزار قدرتمند در مسایل فیلترسازی تبدیل کرده است.