‫آشنايي با فيلتر كالمن‬

‫سعيد عبادالهي‬
‫‪s_ebadollahi@iust.ac.ir‬‬

‫‪٤٠ /‬‬
 ‫فهرست مطالب‬

‫فيلتر كالمن خطي‬ ‫‪٥‬‬ ‫تعريف فيلتر كالمن‬ ‫‪١‬‬

‫فيلتر كالمن توسعهيافته‬ ‫‪٦‬‬ ‫صورت مسئله‬ ‫‪٢‬‬

‫فيلتر كالمن بياثر‬ ‫‪٧‬‬ ‫كاربردهاي فيلتر كالمن‬ ‫‪٣‬‬

‫جمعبندي‬ ‫‪٨‬‬ ‫نگاه كلي به فيلتر كالمن‬ ‫‪٤‬‬

‫‪٤٠ / ٢‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫تعريف فيلتر كالمن‬

‫فيلتر كالمن يك فيلتر بازگشتي بهينه يا يك تخمينگر است‬

‫كـه حالت يك سيستم ديناميكي را از انـدازهگيريهاي نويـزي‬
‫برآورد ميكند‪.‬‬

‫‪٤٠ / ٣‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫صورت مسئله‬

‫دردسترس اندازهگيري ‪Text‬‬

‫عدم دسترسي براي اندازهگيري ‪Tin‬‬

‫‪٤٠ / ٤‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫صورت مسئله‬

‫شتاب و سرعت زاويه خودرو‬ ‫سرعت نسبي خودرو‬ ‫موقعيت مطلق خودرو‬

‫‪٤٠ / ٥‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫صورت مسئله‬

‫عدم كارايي ‪ GPS‬در‬

‫مكانهاي سرپوشيده‬

‫‪٤٠ / ٦‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫صورت مسئله‬

‫موقعيت نسبي خودرو‬ ‫موقعيت مطلق خودرو‬

‫نرخ اندازهگيري باﻻ‬ ‫نرخ اندازهگيري پايين‬

‫متأثر از دريفت‬ ‫متأثر از نويز اندازهگيري زياد‬

‫‪٤٠ / ٧‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫كاربردهاي فيلتر كالمن‬

‫دردسترس اندازهگيري ‪Text‬‬

‫عدم دسترسي براي اندازهگيري ‪Tin‬‬

‫تخمين بهتر حالتهاي سيستم با استفاده‬ ‫تخمين حالتهايي از سيستم كه قابل‬

‫از چندين سنسور!‬ ‫اندازهگيري نيستند!‬

‫‪٤٠ / ٨‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫نگاه كلي به فيلتر كالمن‬

‫‪٤٠ / ٩‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫نگاه كلي به فيلتر كالمن‬

‫‪٤٠ / ١٠‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫منابع خطا‬ ‫منابع خطاي اندازه گيري‬

‫اندازهگيريهاي‬ ‫تخمين بهينه از‬

‫ورودي سيستم‬ ‫وسيله‬ ‫نويزي‬ ‫حالتهاي سيستم‬
‫سيستم‬ ‫فيلتر كالمن‬
‫اندازهگيري‬

‫جايگاه قرارگيري فيلتر كالمن‬

‫نگاه كلي به فيلتر كالمن‬

‫‪٤٠ / ١١‬‬ ‫آشنايي با فيلتر كالمن‬
 ‫فيلتر كالمن خطي‬

‫ معادﻻت تفاضلي يك سيستم خطي دلخواه و مدل مشاهدات‬

 xk  Ak 1 xk 1  Bk 1uk 1  wk 1 ‫عدم قطعيت در مدل‬ μ0  E  x0 

 zk  H k xk  vk

P0  E  x0  μ0  x  
T
‫نويز اندازهگيري‬  μ0
 0

‫ماتريس كوواريانس شرايط اوليه‬
‫بردار حالت اوليه‬

E  wk   0 , E  w k wkT   Qk , E  wk w Tj   0 for k  j , E  w k x0T   0 for all k

E  v k   0 , E  v k v Tk   Rk , E  v k v Tj   0 for k  j , E  v k x0T   0 for all k

٤٠ / ١٢ ‫آشنايي با فيلتر كالمن‬

 ‫فيلتر كالمن خطي‬

‫مرحله پيشبيني‬ ‫مرحله بروزرساني‬

xˆ k  Ak 1 xˆ k 1  Bk 1uk 1 xˆ k  xˆ k  K k  zk  H k xˆ k 
K k  Pk H kT  H k Pk H kT  Rk 
1

Pk  Ak 1 Pk 1 AkT1  Qk 1 Pk   I  K k H k  Pk

‫مدل سيستم و مشاهدات‬

 xk  Ak 1 xk 1  Bk 1uk 1  wk 1

 zk  H k xk  vk

٤٠ / ١٣ ‫آشنايي با فيلتر كالمن‬

 ‫فيلتر كالمن خطي‬

MATLAB

x_p_k = A*x_h_k0;
P_p_k = A*P_k0*A'+Q;
K_k = P_p_k*H'/(H*P_p_k*H'+R);
x_h_k = x_p_k+K_k*(z_k-H*x_p_k);
P_k = (eye(n)-K_k*H)*P_p_k;

x_h_k0 = x_h_k;
P_k0 = P_k;

x_h(:,k) = x_h_k;
trP(k) = trace(P_k);
MSE(k) = ((k-1)*MSE(k-1)+(x_h_k-x_k)'*(x_h_k-x_k))/k;

٤٠ / ١٤ ‫آشنايي با فيلتر كالمن‬

 ‫نمودار بلوكي فيلتر كالمن خطي‬

‫فيلتر كالمن خطي‬

‫‪٤٠ / ١٥‬‬ ‫آشنايي با فيلتر كالمن‬
 ‫مثال‪ .‬تخمين حالت سيستم خطي – اندازهگيري ارتفاع مخزن آب‬ ‫؟‬
‫‪ :‬مدل فرآيند‬ ‫‪xk  xk 1  wk 1‬‬

‫‪ :‬مدل اندازهگيري‬ ‫‪zk  2  xk  vk‬‬

‫‪ ٢‬متر‬ ‫‪zk  2  zk‬‬

‫‪zk  xk  vk‬‬

‫مدل سيستم و مشاهدات‬

‫‪ :‬شرايط اوليه‬ ‫‪x0  0 , P0  1000‬‬
‫‪ xk  Ak 1 xk 1  Bk 1uk 1  wk 1‬‬
‫‪‬‬
‫‪ zk  H k xk  vk‬‬

‫‪٤٠ / ١٦‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫ادامه مثال ارتفاع مخزن ‪...‬‬ ‫؟‬
‫‪ xk  xk 1  wk 1‬‬ ‫‪w  N  0,10 6   Q  106‬‬
‫‪‬‬
‫‪ zk  xk  vk‬‬
‫‪v  N  0,0.01  Rk  0.01‬‬

‫‪ v2  var  z   0.01‬‬

‫نمونه اندازهگيريهاي سنسور‬ ‫هيستوگرام اندازهگيريهاي سنسور‬

‫‪٤٠ / ١٧‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫ادامه مثال ارتفاع مخزن ‪...‬‬ ‫؟‬

‫تخمين ارتفاع آب در مخزن با استفاده از فيلتر كالمن‬

‫‪٤٠ / ١٨‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫ تخمين حالت سيستم غيرخطي – پاندول ساده‬.‫مثال‬ ‫؟‬
 x1  x2
x1 

ml   mg sin   kl 
x2 
 g k
x
 2   sin x1  x2
l m
 l
 x1 ( k  1)  x1 ( k )  Tx2 (k )
x(k  1)  x(k ) 
x   g  k
T x
 2 ( k  1)   T sin x1 ( k )   1  T  x2 (k )
 l  m
mg
z ( k )  x1 ( k )

٤٠ / ١٩ ‫آشنايي با فيلتر كالمن‬

 ... ‫ادامه مثال پاندول‬ ‫؟‬
 x1 ( k  1)  x1 ( k )  Tx2 ( k )  w1 ( k )

 g  k
x
 2 ( k  1)   T sin x1 ( k )   1  T  x2 (k )  w2 ( k )
 l  m

z ( k )  x1 (k )  v ( k )

٤٠ / ٢٠ ‫آشنايي با فيلتر كالمن‬

 ١‫توسعهيافته‬ ‫فيلتر كالمن‬

‫ معادﻻت تفاضلي يك سيستم غيرخطي دلخواه و مدل مشاهدات آن با نويز جمعشونده‬

 xk  f  xk 1   wk 1 ‫عدم قطعيت در مدل‬ μ0  E  x0 

 zk  h  xk   vk

P0  E  x0  μ0  x  
T
‫نويز اندازهگيري‬  μ0
 0

‫ماتريس كوواريانس شرايط اوليه‬
‫بردار حالت اوليه‬

E  wk   0 , E  w k wkT   Qk , E  wk w Tj   0 for k  j , E  w k x0T   0 for all k

E  v k   0 , E  v k v Tk   Rk , E  v k v Tj   0 for k  j , E  v k x0T   0 for all k
1. Extended Kalman Filter

٤٠ / ٢١ ‫آشنايي با فيلتر كالمن‬

 ‫فيلتر كالمن توسعهيافته‬

‫مرحله پيشبيني‬ ‫مرحله بروزرساني‬

xˆ k  f ( xˆ k 1 ) xˆ k  xˆ k  K k  zk  h( xˆ k ) 
K k  Pk H T  HPk H T  R 
1

Pk  Fk 1 Pk 1FkT1  Qk 1 Pk   I  K k H  Pk

‫ژاكوبي تابع فرآيند‬ ‫ژاكوبي تابع اندازهگيري‬

 f 1 f 1 f 1   h1 h1 h1 
 x   ‫مدل سيستم و مشاهدات‬
x 2 x n   x x 2 x n 
 1   1 
F      H       xk  f  xk 1   wk 1
 f f n   h hm 

 n
f n
   m
hm
   zk  h  xk   vk
 x 1 x 2 x n   x 1 x 2 x n 

٤٠ / ٢٢ ‫آشنايي با فيلتر كالمن‬

 ‫نمودار بلوكي فيلتر كالمن توسعهيافته‬

‫فيلتر كالمن توسعهيافته‬

‫‪٤٠ / ٢٣‬‬ ‫آشنايي با فيلتر كالمن‬
 ‫فيلتر كالمن توسعهيافته‬

MATLAB

x_p_k = f(x_h_k0);
P_p_k = Jf(x_h_k0)*P_k0*Jf(x_h_k0)'+Q;
K_k = P_p_k*Jh(x_p_k)'/(Jh(x_p_k)*P_p_k*Jh(x_p_k)'+R);
x_h_k = x_p_k+K_k*(z_k-h(x_p_k));
P_k = (eye(n)-K_k*Jh(x_p_k))*P_p_k;

x_h_k0 = x_h_k;
P_k0 = P_k;

x_h(:,k) = x_h_k;
trP(k) = trace(P_k);
MSE(k) = ((k-1)*MSE(k-1)+(x_h_k-x_k)'*(x_h_k-x_k))/k;

٤٠ / ٢٤ ‫آشنايي با فيلتر كالمن‬

 ١‫بياثر‬ ‫فيلتر كالمن‬

𝑙×1
‫ معادﻻت تفاضلي يك سيستم غيرخطي دلخواه و مدل مشاهدات آن با نويز جمعشونده‬

 xk  f  xk 1 , uk 1   wk 1 ‫عدم قطعيت در مدل‬ μ0  E  x0 


 zk  h  xk   vk

P0  E  x0  μ0  x  
T
‫نويز اندازهگيري‬  μ0
 0

‫ماتريس كوواريانس شرايط اوليه‬
‫بردار حالت اوليه‬

E  wk   0 , E  w k wkT   Qk , E  wk w Tj   0 for k  j , E  w k x0T   0 for all k

E  v k   0 , E  v k v Tk   Rk , E  v k v Tj   0 for k  j , E  v k x0T   0 for all k
1. Unscented Kalman Filter

٤٠ / ٢٥ ‫آشنايي با فيلتر كالمن‬

 ‫فيلتر كالمن بياثر‬

‫‪   2 l     l‬‬
‫‪ ‬تعريف پارامترهاي ثابت‬
‫‪  l‬‬
‫‪g T  1  11l‬‬ ‫‪0.0001 < 𝛼 < 1‬‬ ‫‪ 𝛼 ‬ميزان پراكندگي نقاط حول 𝐱 را نشان ميدهد‪.‬‬
‫‪ ‬‬ ‫به گونه اي است كه بين صفر تا‬ ‫‪ ‬انتخاب‬
‫‪ l  ‬‬ ‫‪i0‬‬
‫) ‪wi( m‬‬ ‫‪‬‬
‫‪‬‬
‫‪1‬‬
‫‪i  1, , 2l‬‬ ‫براي توزيع گوسي معموﻻً ‪ ٢‬انتخاب ميشود‪.‬‬ ‫‪‬‬
‫‪ 2  l   ‬‬
‫‪ ‬‬
‫‪ l    1     ‬‬ ‫‪i0‬‬
‫‪2‬‬ ‫‪2‬‬

‫‪wi(c)  ‬‬
‫‪1‬‬
‫‪‬‬ ‫‪i  1, , 2l‬‬
‫‪ 2  l   ‬‬

‫‪٤٠ / ٢٦‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫فيلتر كالمن بياثر‬

‫‪٤٠ / ٢٧‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫فيلتر كالمن بياثر‬

χ k 1   xˆ k 1 xˆ k 1 g T   Pk 1 xˆ k 1 g T   Pk 1  ‫ خﻼصه روابط پيشبيني و بروزرساني‬

χ k  f  χ k 1 , uk 1 
2l
P yy  ,k  w
T
2l (c )
Y 
 yˆ  Y
 
 yˆ   R

xˆ k   wi( m ) χ i,k i 0
i i ,k k i ,k k

i 0 2l
Pxy ,k  w i(c )  χ i,k  xˆ k  Y i ,k  yˆ k 
T
2l
Pk   wi( c )  χ i,k  xˆ k   χ i,k  xˆ   Q
 T
k i 0
i 0
K k  Pxy ,k Pyy1,k
Y  h χ
 

 
k k
2l
xˆ k  xˆ k  K k y k  yˆ k
yˆ   w Y
 (m) 
k
i 0
i i ,k
Pk  Pk  K k P yy  ,k K kT

٤٠ / ٢٨ ‫آشنايي با فيلتر كالمن‬

 ‫ادامه مثال پاندول ‪...‬‬ ‫؟‬

‫تخمين زاويه پاندول با فيلتر كالمن توسعه يافته‬

‫‪٤٠ / ٢٩‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫ادامه مثال پاندول ‪...‬‬ ‫؟‬

‫مقايسه ميانگين مربعات خطاي تخمين با انواع فيلتر كالمن‬

‫‪٤٠ / ٣٠‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫مثال‪ .‬تخمين وضعيت‬ ‫؟‬
‫‪ ‬وضعيت يك جسم‪ ،‬در واقع حالت يا جهتگيري دستگاه بدنه تعريفشده بر روي جسم نسبت به‬
‫يك دستگاه مرجع مشخص پايدار مثل دستگاه اينرسي يا يك جسم مشخص مثل زمـين تعريـف‬
‫ميشود‪.‬‬

‫زواياي اويلر‬

‫‪٤٠ / ٣١‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫مثال‪ .‬تخمين وضعيت‬ ‫؟‬
‫زواياي اويلر‬
‫‪‬‬ ‫‪‬‬ ‫‪‬‬
‫زاويه چرخش‬ ‫زاويه فراز‬ ‫زاويه سمت‬
‫‪Roll / Bank angle‬‬ ‫‪Pitch / Elevation angle‬‬ ‫‪Yaw / Azimuth angle‬‬

‫‪٤٠ / ٣٢‬‬ ‫آشنايي با فيلتر كالمن‬

 ... ‫ادامه مثال تخمين وضعيت‬ ‫؟‬
‫استفاده از شتابسنج و مغناطيسسنج‬ ‫انتگرالگيري از ژيروسكوپ‬

inc  atan2   f y ,  f z  Θk 1  Θk  Ts A  Θk   nb
b
,k

inc  arcsin  f x 
1 sin k tan  k cos k tan  k 
A  Θk   0 cos k  sin k 
 cmp  atan2  mty , mxt   D  
0 sin k sec k cos k sec k 
Θk  k ,  k ,  k 
T

٤٠ / ٣٣ ‫آشنايي با فيلتر كالمن‬

 ‫ادامه مثال تخمين وضعيت ‪...‬‬ ‫؟‬

‫چرا تركيب اطﻼعات سنسورها ؟‬

‫‪ ‬رفع مشكل دريفت در تعيين وضعيت با ژيروسكوپ‬

‫‪ ‬كاهش هزينه‪ ،‬حجم و وزن سيستم‬
‫‪ ‬افزايش دقت و كاهش اثر نويز و اغتشاش‬
‫‪ ‬رفع مشكل ندانستن شرايط اوليه‬

‫‪٤٠ / ٣٤‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫ادامه مثال تخمين وضعيت ‪...‬‬ ‫؟‬
‫تركيب اطﻼعات با فيلتر كالمن‬

‫‪ nbb‬‬
‫ژيروسكوپ‬

‫‪Θ‬‬ ‫̂‪Θ‬‬
‫شتاب سنج‬ ‫ترازسنج‬ ‫الگوريتم ‪١‬‬ ‫وضعيت‬
‫‪Θmes‬‬ ‫‪EKF/UKF‬‬

‫مغناطيسسنج‬
‫قطب نما‬

‫‪٤٠ / ٣٥‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫ادامه مثال تخمين وضعيت ‪...‬‬ ‫؟‬
‫نمودار تخمين زاويه چرخش‬

‫‪٤٠ / ٣٦‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫ادامه مثال تخمين وضعيت ‪...‬‬ ‫؟‬
‫نمودار ميانگين مربعات خطاي تخمين وضعيت‬
‫خطاي انتگرالگيري با ژروسكوپ‬

‫خطاي شتابسنج و مغناطيسسنج‬

‫‪٤٠ / ٣٧‬‬ ‫آشنايي با فيلتر كالمن‬

 ‫جمع بندي‬

‫‪ ‬مراحل استفاده از فيلتر كالمن‪:‬‬ ‫‪ ‬چه موقع از فيلتر كالمن استفاده ميكنيم؟‬

‫‪ o‬استخراج مدل ديناميكي فرآيند‬ ‫‪ o‬تخمــين حالتهــايي از سيســتم كــه قابــل‬

‫‪ o‬استخراج مدل اندازهگيري‬ ‫اندازهگيري نيستند!‬

‫‪ o‬تعيين پارامترهاي مدل‬ ‫‪ o‬تخمين بهتر حالتهاي سيستم با استفاده از‬

‫چندين سنسور!‬
‫‪ o‬پياده سازي فيلتر‬

‫‪٤٠ / ٣٨‬‬ ‫آشنايي با فيلتر كالمن‬