{"id":724,"date":"2016-12-01T15:53:27","date_gmt":"2016-12-02T00:53:27","guid":{"rendered":"\/blog\/?p=724"},"modified":"2023-09-21T09:37:51","modified_gmt":"2023-09-21T00:37:51","slug":"ukf-unscented-kalman-filter-radar-%ec%b6%94%ec%a0%81","status":"publish","type":"post","link":"https:\/\/hasu0707.duckdns.org\/blog\/?p=724","title":{"rendered":"UKF-Unscented Kalman Filter - Radar \ucd94\uc801"},"content":{"rendered":"\n

\ucd9c\ucc98 : http:\/\/blog.naver.com\/chogaehwan\/220773857547<\/a><\/p>\r\n


<\/p>\r\n

\"1.png\"
<\/p>\r\n


<\/p>\r\n\r\n<\/span>

<\/p>\r\n

 <\/span><\/p>\r\n

\uceac\ud558 \ub4dc\ub514\uc5b4 \ud588\ub2e4.<\/span><\/p>\r\n

 <\/span><\/p>\r\n

unscented \uce7c\ub9cc \ud544\ud130\u2026\ubb50\ub77c\uace0 \uc124\uba85\ud574\uc57c \ud560\uae4c..?<\/span><\/p>\r\n

 <\/span><\/p>\r\n

 <\/span><\/p>\r\n

EKF\ub294 \ube44\uc120\ud615\uc744 \uc608\uce21\uac12\uc774\ub098 \uc9c1\uc804\uac12\uc5d0\uc11c \uc120\ud615\ud654\ud558\uc5ec \uce7c\ub9cc\ud544\ud130\ub97c \uc774\uc6a9\ud558\uc600\ub2e4.<\/span><\/p>\r\n

\ud558\uc9c0\ub9cc UKF\ub294 \uc9c1\uc804 \ucd94\uc815\uac12 \uadfc\ucc98 2n+1\uac1c\uc758 \uc0d8\ud50c\ub4e4\uc744 \ucd94\ucd9c\ud558\uc5ec \uac01\uac01 \uac00\uc911\uce58\ub97c \ub450\uc5b4<\/span><\/p>\r\n

\ucd94\uc815\uac12\uacfc \uce21\uc815\uac12\uc744 \uc608\uce21\ud55c\ub2e4. \uac01\uac12\uc5d0\ub294 \ud3c9\uade0\uacfc \ubd84\uc0b0\uc774 \uc788\uac8c \ub41c\ub2e4.<\/span><\/p>\r\n

\uadf8 \uac12\ub4e4\ub85c \ucd94\uc815\uce58\uc640 \uc624\ucc28 \uacf5\ubd84\uc0b0\uc744 \uacc4\uc0b0\ud574\ub0b4\ub294 \uac83\uc774\ub2e4.<\/span><\/p>\r\n

\ud655\ub960 \uce7c\ub9cc \ud544\ud130\ub77c\uace0 \ubcf4\uba74 \uc624\ud788\ub824 \uc774\ud574\uac00 \uac00\uae30 \uc26c\uc6b8 \uac83 \uac19\ub2e4. unscented\uac00 \ubb50\uc57c \u2026stochastic kalman filter\uac00 \ub0ab\uc9c0\u2026<\/span><\/p>\r\n

 <\/span><\/p>\r\n

\ud558\ud2bc \uae30\ubcf8 \uad6c\uc870\ub294 \uc544\ub798\uc640 \uac19\ub2e4.<\/span><\/p>\r\n

\"2.png\"\r\n<\/span><\/p>\r\n


<\/p>\r\n\r\n<\/span>

<\/p>\r\n

\"3.png\"\r\n<\/span><\/p>\r\n

\uba3c\uc800 \uc2dc\uadf8\ub9c8 \ud3ec\uc778\ud2b8\ub97c \ubf51\uc544\ub0b4\ub294 \uc791\uc5c5\u2026chol\uc740 cholesky decomposition<\/span>
<\/p>\r\n

2n+1\uac1c\uc758 \uc2dc\uadf8\ub9c8\ud3ec\uc778\ud2b8\uc640 weight\ub97c \uacc4\uc0b0\ud574\ub0b8\ub2e4.<\/span><\/p>\r\n

\uc774\ub54c \uc911\uc694\ud55c\uac8c x\ub791 P \ucd08\uae30\uac12\uc740 \uc5b4\ub514\uc5d0 \uc788\ub290\ub1e8\u2026.?<\/span><\/p>\r\n

\"4.png\"\r\n<\/span><\/p>\r\n

\uc694\uac70 \uba54\ubaa8\ub9ac\ub97c \uc5f4\uc5b4\ubcf4\uba74<\/span>
<\/p>\r\n

\"5.png\"\r\n<\/span><\/p>\r\n


<\/p>\r\n\r\n<\/span>

<\/p>\r\n

\uc774\ub807\uac8c \ucd08\uae30\uac12 \uc124\uc815 \ud560 \uc218 \uc788\uac8c \ud574\uc900\ub2e4.<\/span><\/p>\r\n

<\/colgroup>

1<\/span><\/p>\r\n

2<\/span><\/p>\r\n

3<\/span><\/p>\r\n

4<\/span><\/p>\r\n

5<\/span><\/p>\r\n

6<\/span><\/p>\r\n

7<\/span><\/p>\r\n

8<\/span><\/p>\r\n

9<\/span><\/p>\r\n

10<\/span><\/p>\r\n

11<\/span><\/p>\r\n

12<\/span><\/p>\r\n

13<\/span><\/p>\r\n

14<\/span><\/p>\r\n

15<\/span><\/p>\r\n

16<\/span><\/p>\r\n

17<\/span><\/p>\r\n

18<\/span><\/p>\r\n

19<\/span><\/p>\r\n

20<\/span><\/p>\r\n

21<\/span><\/p>\r\n

22<\/span><\/p>\r\n

23<\/span><\/p>\r\n

24<\/span><\/p>\r\n

25<\/span><\/p>\r\n

26<\/span><\/p>\r\n

27<\/span><\/p>\r\n

28<\/span><\/p>\r\n

29<\/span><\/p>\r\n

30<\/span><\/p>\r\n

31<\/span><\/p>\r\n

32<\/span><\/p>\r\n

33<\/span><\/p>\r\n

34<\/span><\/p>\r\n

35<\/span><\/p>\r\n

36<\/span><\/p>\r\n

37<\/span><\/p>\r\n

38<\/span><\/p>\r\n

39<\/span><\/p>\r\n

40<\/span><\/p>\r\n

41<\/span><\/p>\r\n

42<\/span><\/p>\r\n

43<\/span><\/p>\r\n

44<\/span><\/p>\r\n

45<\/span><\/p>\r\n

46<\/span><\/p>\r\n

47<\/span><\/p>\r\n

48<\/span><\/p>\r\n

49<\/span><\/p>\r\n

50<\/span><\/p>\r\n

51<\/span><\/p>\r\n

52<\/span><\/p>\r\n

53<\/span><\/p>\r\n

54<\/span><\/p>\r\n

55<\/span><\/p>\r\n<\/td>

function [K,xp,zp,Pp,Pz] <\/span>=<\/span> UT(X,W)<\/span><\/span><\/span><\/p>\r\n

   <\/span><\/p>\r\n

% state variable prediction<\/span><\/p>\r\n

dt<\/span>=<\/span>0.05<\/span>;<\/span><\/span><\/span><\/span><\/p>\r\n

n<\/span>=<\/span>3<\/span>;<\/span><\/span><\/span><\/span><\/p>\r\n

fX<\/span>=<\/span>zeros(n,<\/span>2<\/span>*<\/span>n<\/span>+<\/span>1<\/span>);<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\r\n

xp<\/span>=<\/span>zeros(n,<\/span>1<\/span>);<\/span><\/span><\/span><\/span><\/span><\/p>\r\n

Pp<\/span>=<\/span>zeros(n,n);<\/span><\/span><\/span><\/p>\r\n

Q<\/span>=<\/span>0.001<\/span>*<\/span>eye(<\/span>3<\/span>);<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\r\n

   <\/span><\/p>\r\n

for<\/span> i<\/span>=<\/span>1<\/span>:<\/span>2<\/span>*<\/span>n<\/span>+<\/span>1<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\r\n

    fX(:,i)<\/span>=<\/span>(eye(<\/span>3<\/span>)<\/span>+<\/span>[<\/span>0<\/span> <\/span>0<\/span> dt;<\/span>0<\/span> <\/span>0<\/span> <\/span>0<\/span>;<\/span>0<\/span> <\/span>0<\/span> <\/span>0<\/span>])<\/span>*<\/span>X(:,i);<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\r\n

end<\/span><\/span><\/span><\/p>\r\n

   <\/span><\/p>\r\n

for<\/span> i<\/span>=<\/span>1<\/span>:<\/span>2<\/span>*<\/span>n<\/span>+<\/span>1<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\r\n

    xp<\/span>=<\/span>xp<\/span>+<\/span>W(i)<\/span>*<\/span>fX(:,i);<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\r\n

end<\/span><\/span><\/span><\/p>\r\n

   <\/span><\/p>\r\n

for<\/span> i<\/span>=<\/span>1<\/span>:<\/span>2<\/span>*<\/span>n<\/span>+<\/span>1<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\r\n

    Pp<\/span>=<\/span>Pp<\/span>+<\/span>W(i)<\/span>*<\/span>(fX(:,i)<\/span>-<\/span>xp)<\/span>*<\/span>(fX(:,i)<\/span>-<\/span>xp)<\/span>';<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\r\n

end<\/span><\/span><\/span><\/p>\r\n

Pp=Pp+Q;<\/span><\/span><\/span><\/p>\r\n

% measured value prediction<\/span><\/span><\/span><\/p>\r\n

m=1;<\/span><\/span><\/span><\/p>\r\n

hX=zeros(m,2*n+1);<\/span><\/span><\/span><\/p>\r\n

zp=zeros(m,1);<\/span><\/span><\/span><\/p>\r\n

Pz=zeros(m,m);<\/span><\/span><\/span><\/p>\r\n

R=100;<\/span><\/span><\/span><\/p>\r\n

for i=1:2*n+1<\/span><\/span><\/span><\/p>\r\n

    hX(:,i)=sqrt(X(1,i)^2+X(2,i)^2);<\/span><\/span><\/span><\/p>\r\n

end<\/span><\/span><\/span><\/p>\r\n

for i=1:2*n+1<\/span><\/span><\/span><\/p>\r\n

    zp=zp+W(i)*hX(:,i);<\/span><\/span><\/span><\/p>\r\n

end<\/span><\/span><\/span><\/p>\r\n

for i=1:2*n+1<\/span><\/span><\/span><\/p>\r\n

    Pz=Pz+W(i)*(hX(:,i)-zp)*(hX(:,i)-zp)'<\/span>;<\/span><\/span><\/p>\r\n

end<\/span><\/span><\/span><\/p>\r\n

   <\/span><\/p>\r\n

Pz<\/span>=<\/span>Pz<\/span>+<\/span>R;<\/span><\/span><\/span><\/span><\/span><\/p>\r\n

   <\/span><\/p>\r\n

% calculate kalman gain<\/span><\/p>\r\n

Pxz<\/span>=<\/span>zeros(n,m);<\/span><\/span><\/span><\/p>\r\n

K<\/span>=<\/span>zeros(n,m);<\/span><\/span><\/span><\/p>\r\n

   <\/span><\/p>\r\n

for<\/span> i<\/span>=<\/span>1<\/span>:<\/span>2<\/span>*<\/span>n<\/span>+<\/span>1<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\r\n

    Pxz<\/span>=<\/span>Pxz<\/span>+<\/span>W(i)<\/span>*<\/span>(fX(:,i)<\/span>-<\/span>xp)<\/span>*<\/span>(hX(:,i)<\/span>-<\/span>zp)<\/span>';<\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/span><\/p>\r\n

end<\/span><\/span><\/span><\/p>\r\n

K=Pxz*inv(Pz);<\/span><\/span><\/span><\/p>\r\n

Colored by Color Scripter<\/span><\/em><\/span><\/a><\/p>\r\n<\/td>

cs<\/span><\/a><\/p>\r\n<\/td><\/tr><\/tbody><\/table><\/div>

\uadf8\ub2f4 \uc774\uc81c fX\ub791 hX \uacc4\uc0b0\ud574\uc11c K,xp,zp,Pp,Pz\ub97c \uacc4\uc0b0\ud558\ub294 \uac83\uc774\ub2e4.<\/span><\/p>\r\n

\uc5ec\uae30\uc11c fX\ub294 xp\uacc4\uc0b0\uc758 \ud6c4\ubcf4\uad70\uc774 \ub418\ub294 \ud3ec\uc778\ud2b8\ub4e4\uc774\ub2e4.<\/span><\/p>\r\n

hX\ub294 zp\uc758 \ud6c4\ubcf4\uad70\uc774 \ub418\ub294 \ud3ec\uc778\ud2b8\ub4e4\uc778 \uac83\uc774\ub2e4. \uac01\uac01\uc758 \uc2dc\uadf8\ub9c8 \ud3ec\uc778\ud2b8\ub4e4\ub85c \uacc4\uc0b0\ud558\ub294 \uac83\uc774\ub2e4.<\/span><\/p>\r\n

Px+Q\ub791 Pz+R \ud574\uc8fc\uc2dc\uace0\u2026\uacc4\uc0b0\ud558\uba74 \ub41c\ub2e4.<\/span><\/p>\r\n

\"6.png\"\r\n<\/span><\/p>\r\n

\ub9c8\uc9c0\ub9c9\uc73c\ub85c \uce21\uc815\uac12\uc744 \ucc38\uace0\ud558\uc5ec \uacc4\uc0b0\ud558\uba74 \ub41c\ub2e4.<\/span>
<\/p>\r\n

\"7.png\"\r\n<\/span><\/p>\r\n

\uacb0\uacfc\ub294 \ubb3c\ub860 \uc544\uc8fc \uc798 \ub098\uc628\ub2e4.<\/span>
<\/p>\n","protected":false},"excerpt":{"rendered":"

\ucd9c\ucc98 : http:\/\/blog.naver.com\/chogaehwan\/220773857547   \uceac\ud558 \ub4dc\ub514\uc5b4 \ud588\ub2e4.   unscented \uce7c\ub9cc \ud544\ud130\u2026\ubb50\ub77c\uace0 \uc124\uba85\ud574\uc57c \ud560\uae4c..?     EKF\ub294 \ube44\uc120\ud615\uc744 \uc608\uce21\uac12\uc774\ub098 \uc9c1\uc804\uac12\uc5d0\uc11c \uc120\ud615\ud654\ud558\uc5ec \uce7c\ub9cc\ud544\ud130\ub97c \uc774\uc6a9\ud558\uc600\ub2e4. \ud558\uc9c0\ub9cc UKF\ub294 \uc9c1\uc804 \ucd94\uc815\uac12 \uadfc\ucc98 2n+1\uac1c\uc758 \uc0d8\ud50c\ub4e4\uc744 \ucd94\ucd9c\ud558\uc5ec \uac01\uac01 \uac00\uc911\uce58\ub97c \ub450\uc5b4 \ucd94\uc815\uac12\uacfc \uce21\uc815\uac12\uc744 \uc608\uce21\ud55c\ub2e4. \uac01\uac12\uc5d0\ub294 \ud3c9\uade0\uacfc \ubd84\uc0b0\uc774 \uc788\uac8c \ub41c\ub2e4. \uadf8 \uac12\ub4e4\ub85c \ucd94\uc815\uce58\uc640 \uc624\ucc28 \uacf5\ubd84\uc0b0\uc744 \uacc4\uc0b0\ud574\ub0b4\ub294 \uac83\uc774\ub2e4. \ud655\ub960 \uce7c\ub9cc \ud544\ud130\ub77c\uace0 \ubcf4\uba74 \uc624\ud788\ub824 \uc774\ud574\uac00 \uac00\uae30 \uc26c\uc6b8 \uac83 […]<\/p>\n","protected":false},"author":2,"featured_media":0,"comment_status":"closed","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"_import_markdown_pro_load_document_selector":0,"_import_markdown_pro_submit_text_textarea":"","site-sidebar-layout":"default","site-content-layout":"","ast-site-content-layout":"","site-content-style":"default","site-sidebar-style":"default","ast-global-header-display":"","ast-banner-title-visibility":"","ast-main-header-display":"","ast-hfb-above-header-display":"","ast-hfb-below-header-display":"","ast-hfb-mobile-header-display":"","site-post-title":"","ast-breadcrumbs-content":"","ast-featured-img":"","footer-sml-layout":"","theme-transparent-header-meta":"","adv-header-id-meta":"","stick-header-meta":"","header-above-stick-meta":"","header-main-stick-meta":"","header-below-stick-meta":"","astra-migrate-meta-layouts":"default","ast-page-background-enabled":"default","ast-page-background-meta":{"desktop":{"background-color":"","background-image":"","background-repeat":"repeat","background-position":"center center","background-size":"auto","background-attachment":"scroll","background-type":"","background-media":"","overlay-type":"","overlay-color":"","overlay-opacity":"","overlay-gradient":""},"tablet":{"background-color":"","background-image":"","background-repeat":"repeat","background-position":"center center","background-size":"auto","background-attachment":"scroll","background-type":"","background-media":"","overlay-type":"","overlay-color":"","overlay-opacity":"","overlay-gradient":""},"mobile":{"background-color":"","background-image":"","background-repeat":"repeat","background-position":"center center","background-size":"auto","background-attachment":"scroll","background-type":"","background-media":"","overlay-type":"","overlay-color":"","overlay-opacity":"","overlay-gradient":""}},"ast-content-background-meta":{"desktop":{"background-color":"var(--ast-global-color-5)","background-image":"","background-repeat":"repeat","background-position":"center center","background-size":"auto","background-attachment":"scroll","background-type":"","background-media":"","overlay-type":"","overlay-color":"","overlay-opacity":"","overlay-gradient":""},"tablet":{"background-color":"var(--ast-global-color-5)","background-image":"","background-repeat":"repeat","background-position":"center center","background-size":"auto","background-attachment":"scroll","background-type":"","background-media":"","overlay-type":"","overlay-color":"","overlay-opacity":"","overlay-gradient":""},"mobile":{"background-color":"var(--ast-global-color-5)","background-image":"","background-repeat":"repeat","background-position":"center center","background-size":"auto","background-attachment":"scroll","background-type":"","background-media":"","overlay-type":"","overlay-color":"","overlay-opacity":"","overlay-gradient":""}},"footnotes":""},"categories":[29],"tags":[],"class_list":["post-724","post","type-post","status-publish","format-standard","hentry","category-multi_copter_base"],"_links":{"self":[{"href":"https:\/\/hasu0707.duckdns.org\/blog\/index.php?rest_route=\/wp\/v2\/posts\/724","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/hasu0707.duckdns.org\/blog\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/hasu0707.duckdns.org\/blog\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/hasu0707.duckdns.org\/blog\/index.php?rest_route=\/wp\/v2\/users\/2"}],"replies":[{"embeddable":true,"href":"https:\/\/hasu0707.duckdns.org\/blog\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=724"}],"version-history":[{"count":0,"href":"https:\/\/hasu0707.duckdns.org\/blog\/index.php?rest_route=\/wp\/v2\/posts\/724\/revisions"}],"wp:attachment":[{"href":"https:\/\/hasu0707.duckdns.org\/blog\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=724"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/hasu0707.duckdns.org\/blog\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=724"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/hasu0707.duckdns.org\/blog\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=724"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}