主要内容

이번역페이지는최신내용을담고있지않습니다。최신내용을영문으로보려면여기를클릭하십시오。

시변칼만필터를사용한상태추정

이예제에서는仿真金宝app软件에서시변칼만필터를사용하여선형시스템의상태를추정하는방법을보여줍니다。控制系统工具箱라이브러리에서卡尔曼滤波器블록을사용하여잡음이있는위치측정값(예:GPS센서측정값)을기반으로지상차량의위치와속도를추정합니다。칼만필터의플랜트모델은시변잡음특성을갖습니다。

소개

북쪽과동쪽방향에서지상차량의위치와속도를추정하려고합니다。차량은2차원공간에서아무제약없이자유롭게이동할수있습니다。차량뿐아니라모든물체에사용할수있는다목적내비게이션및위치추적시스템을설계합니다。

美元美元x_e (t)x_n (t)美元는원점으로부터차량의동쪽및북쪽위치이고\θ(t)美元는동쪽에서차량방향이며u_ \ psi (t)美元는차량의조향각입니다。元新台币는연속시간변수입니다。

金宝app仿真软件모델은두가지주요부분,즉차량모델과칼만필터로구성됩니다。이에대해서는다음섹션에서자세히설명합니다。

open_system (“ctrlKalmanNavigationExample”);

차량모델

추적하는차량은다음과같은간단한점질량(质点)모델로표현됩니다。

$ $ \压裂{d} {dt} \离开[& # xA; \开始{数组}{c} & # xA;x_e (t) \ \ & # xA;x_n (t) \ \ & # xA;s (t) \ \ & # xA;\θ(t) & # xA; \{数组}\]= \左右结束[& # xA; \开始{数组}{c} & # xA;s (t) \ cosθ(t)) (\ \ \ & # xA;s (t) \ sinθ(t)) (\ \ \ & # xA;(P \;{u_T(t)}{s(t)} - A \;重金属镉\; s(t)^2) / m \\
 s(t) \tan(u_\psi(t)) / L
\end{array} \right]
$$

여기서차량상태는다음과같습니다。

$$ \begin{array} {all}
x_e(t) \;& # 38;\ textnormal{东位置}\;[m] \ \ & # xA; x_n (t) \;& # 38;\ textnormal{北位置}\;[m] \ \ & # xA; s (t) \;& # 38;\ textnormal{速度}\;(米/秒)\ \ & # xA; \θ(t) \; & \textnormal{Orientation from east} \; [deg] \\
\end{array} $$

차량파라미터는다음과같습니다。

$$ \begin{array} {ll}
P=100000 \;& # 38;\textnormal{峰值引擎功率}\;[W] \ \ & # xA; A = 1 \;& # 38;\ textnormal{锋面}\;(m ^ 2) \ \ & # xA;重金属镉= 0.3 \;& # 38;\ textnormal{阻力系数}\;(无单位)\ \ & # xA; m = 1250 \; & \textnormal{Vehicle mass} \; [kg] \\
L=2.5 \; & \textnormal{Wheelbase length} \; [m] \\
\end{array} $$

제어입력은다음과같습니다。

$$ \begin{array} {ll}
u_T(t) \;& # 38;\textnormal{油门位置在-1和1的范围内}\;(无单位)\ \ & # xA; u_ \ psi (t) \;& # 38;\ textnormal{转向角}\;[度]\ \ & # xA; \{数组}$ $

이모델의종방향역학에서는구름저항(滚动阻力)을무시합니다。이모델의횡방향역학에서는원하는조향각에즉시도달할수있는것으로가정하며요(偏航)관성모멘트를무시합니다。

차량모델은ctrlKalmanNavigationExample /车辆模型서브시스템에구현되어있습니다。이仿金宝app真软件모델의ctrlKalmanNavigationExample/速度和方向跟踪서브시스템에는차량의원하는방향과속도를추종하기위한두개의π제어기가포함되어있습니다。따라서차량에다양한기동조건을지정하여칼만필터의성능을테스트할수있습니다。

칼만필터설계

칼만필터는선형모델을기반으로미지의관심변수를추정하는알고리즘입니다。이선형모델은모델초기조건뿐아니라알려진모델입력과알수없는모델입력에대한응답으로시간에따른추정된변수의변화를설명합니다。이예제에서는다음과같은파라미터/변수를추정합니다。

$$ \hat{x}[n] = \left[
c \开始{数组}{}& # xA;{x} \帽子_e [n] \ \ & # xA;{x} \帽子_n”[n] \ \ & # xA;{\ \帽子点{x}} _e [n] \ \ & # xA;{\ \帽子点{x}} _n”[n] & # xA;结束\{数组}& # xA;\右]& # xA; $ $

여기서각각은다음을나타냅니다。

$$ \begin{array} {all}
\hat{x}_e[n] \;& # 38;\textnormal{East position estimate} \;[m] \ \ & # xA; \帽子{x} _n”[n] \;& # 38;\textnormal{北位置估计}\;[m] \ \ & # xA; \帽子{\点{x}} _e [n] \;& # 38;\textnormal{East velocity estimate} \;(米/秒)\ \ & # xA; \帽子{\点{x}} _n”[n] \; & \textnormal{North velocity estimate} \; [m/s] \\
\end{array} $$

$ \点{x} $항은미분연산자가아니라속도를나타냅니다。n美元은이산시간인덱스입니다。칼만필터에사용되는모델의형식은다음과같습니다。

数组$ $ \开始{}{rl} & # xA; \帽子{x} (n + 1) & # 38; = \帽子{x} [n] + Gw [n] \ \ & # xA; y [n] & # 38; = C \帽子{x} [n] + v [n] & # xA; \{数组}$ $

여기서$ \帽子{x} $는상태벡터,y美元는측정값,w美元는공정잡음,五美元는측정잡음입니다。칼만필터는w美元五美元가평균0,알려진분산美元$ E [ww ^ T] =问E (vv ^ T) = R美元美元$ E [wv ^ T] = N인독립적인확률변수라고가정합니다。여기서A、G、C행렬은다음과같으며

$$ A = \left[
\begin{array}{c c c}
1 & # 38岁;0 & # 38;T_s & # 38;0 xA \ \ & #;0 & # 38;1 & # 38岁;0 & # 38;T_s \ \ & # xA; 0 & 0 & 1 & 0 \\
 0 & 0 & 0 & 1
 \end{array}
 \right]
$$

$$ G = \left[
开始\{数组}{c c} & # xA;T_s / 2 & # 38;0 xA \ \ & #;0 & # 38;T_s / 2 \ \ & # xA;1 & # 38岁;0 xA \ \ & #;0 & # 38;1 & # xA; \end{array}
 \right]
$$

$$ C = \left[
\begin{array}{c c c}
1 & # 38岁;0 & # 38;0 & # 38;0 xA \ \ & #;0 & # 38;1 & # 38岁;0 & # 38;0 xA & #; \end{array}
 \right]
$$

美元$ T_s = 1 \; [s]입니다。

와G의세번째행은동쪽속도를임의보행으로모델링합니다($ \帽子{\点{x}} _e (n + 1) ={\点{x}} \帽子_e [n] + w_1美元[n]).실세계에서위치는연속시간변수이며시간에따른속도의적분입니다($ \压裂{d} {dt} {x} _e = \ \帽子帽子{\点{x}} _e美元).와G의첫번째행은이운동학적관계에대한이산근삿값을나타냅니다(美元(\帽子{x} _e (n + 1) - \帽子{x} _e [n]) / Ts =(\帽子{\点{x}} _e (n + 1) + \帽子{\点{x}} _e [n]) / 2美元).와G의두번째및네번째행은북쪽속도와위치간의동일한관계를나타냅니다。

C행렬은위치측정값만사용할수있음을나타냅니다。GPS와같은위치센서는1 hz의샘플레이트에서이러한측정값을제공합니다。측정잡음五美元의분산R행인렬은R = 50美元으로지정됩니다。R은스칼라로지정되므로卡尔曼滤波器블록은행렬R이대각행렬이고대각선요소는50이며y와호환되는차원을갖는것으로가정합니다。측정잡음이가우스분포이면R = 50은위치측정값이동쪽과북쪽방향의실제위치로부터下午\ \ sqrt{50} \美元;m美元이내의거리에속할확률이68%라는의미입니다。그러나칼만필터의경우이가정은불필요합니다。

w美元의요소는차량속도가하나의샘플시간Ts에얼마나변화할수있는지를나타냅니다。공정잡w음의분산인问행렬은시변으로선택되었습니다。이것은속도가크면w [n]美元의일반적인값은더작다는직관을반영합니다。예를들어,0 m / s에서로10 m / s올리는것10 m / s이에서로20 m / s올리는것보다쉽습니다。구체적으로,다음과같이추정된북쪽및동쪽속도와포화함수를사용하여Q [n]을생성합니다。

$ $ f{坐在}(z) = min (max (z, 25), 625) $ $

$$ Q[n] = \left[
\begin{array}{c c}
\ displaystyle 1 + \压裂{250}{f{坐在}(\帽子{\点{x}} _e ^ 2)} & # 38;0 xA \ \ & #;0 & # 38;\ displaystyle 1 + \压裂{250}{f{坐在}({\点{x}} \帽子_n”^ 2)}& # xA;结束\{数组}& # xA;\右]& # xA; $ $

问의대각선요소는추정된속도의제곱에반비례하는w의분산을모델링합니다。Q포화함수는가너무크거나너무작지않도록방지합니다。250은계수일반차량의0 ~ 5,5 ~ 10,10 ~ 15日,15 ~ 20,20 ~ 25米/秒가속시간데이터에최소제곱피팅을사용하여구한것입니다。대각선问에대한선택은북쪽과동쪽방향의속도변화가상관관계가없다고단순하게가정했음을나타냅니다。

卡尔曼滤波器블록입력및설정

卡尔曼滤波器的블록은仿真软件의金宝app控制系统工具箱라이브러리에있습니다。이블록은또한系统辨识工具箱/估计라이브러리에도있습니다。이산시간상태추정에대해블록파라미터를구성합니다。다음필터설정파라미터를지정합니다。

  • 시간영역:이산시간。이옵션을선택하면이산시간상태를추정할수있습니다。

  • 현재측정값y [n]을사용하여xhat [n]향상체크박스를선택합니다。그러면이산시간칼만필터의”현재추정기”변형이구현됩니다。이옵션은추정정확도를향상시키며느린샘플시간에더유용합니다。반면,계산비용이증가합니다。또한이칼만필터변형에는직접피드스루가있으며이는칼만필터가어떤지연도포함되지않은피드백루프에사용되는경우대수루프를발생시킵니다(피드백루프자체에도직접피드스루가있음)。대수루프는시뮬레이션속도에도영향을미칠수있습니다。

옵션탭을클릭하여블록인포트및아웃포트옵션을설정합니다。

  • 입력포트u추가체크박스를선택취소합니다。플랜트모델에알려진입력이없습니다。

  • 상태추정오차의공분산Z출력체크박스를선택합니다。Z행렬은필터의상태추정값에대한신뢰성정보를제공합니다。

모델파라미터를클릭하여플랜트모델및잡음특성을지정합니다。

  • 모델소스:개별A, B, C, D행렬。

  • 一个: A .행렬은이예제의앞부분에서정의되었습니다。

  • C: C . C행렬은이예제의앞부분에서정의되었습니다。

  • 초기추정값소스:대화상자

  • 초기상태x [0]0.이값은t = 0에서위치및속도추정값의초기추측값이0임을나타냅니다。

  • 상태추정오차의공분산P [0]10.초기추측값x[0]과실제값간의오차는표준편차가$ \ sqrt {10} $인확률변수라고가정합니다。

  • G행렬및H행렬사용(디폴트값:G =我및H = 0)체크박스를선택하여디폴트가아닌G행렬을지정합니다。

  • G: G G행렬은이예제의앞부분에서정의되었습니다。

  • H0.공정잡음은卡尔曼滤波器블록에들어가는측정값y에영향을미치지않습니다。

  • 시불변问체크박스를선택취소합니다。问행렬은시변행렬이며블록인포트问를통해제공됩니다。이설정으로인해블록은시변칼만필터를사용합니다。이옵션을선택하면시불변칼만필터를사용할수있습니다。시불변칼만필터는이문제에대해성능은약간떨어지지만설계가더쉽고계산비용이낮습니다。

  • R: r .측정잡음v [n]美元의공분산입니다。R행렬은이예제의앞부분에서정의되었습니다。

  • N0.공정잡음과측정잡음간에상관관계가없다고가정합니다。

  • 샘플시간(상속된경우1): Ts。이예제의앞부분에서정의되었습니다。

결과

차량이다음과같이기동하는시나리오를시뮬레이션하여칼만필터의성능을테스트합니다。

  • t = 0에서차량위치는美元x_e(0) = 0美元x_n(0) = 0美元이고정지상태입니다。

  • 25米/秒동쪽방향으로로가속합니다。t = 50年代에서5 m / s로감속합니다。

  • t = 100年代에서북쪽으로방향을돌려20 m / s로가속합니다。

  • t = 200年代에서다시서쪽으로방향을돌립니다。25米/秒로가속합니다。

  • 15 m / s t = 260年代에서로감속하고일정한속도로180도방향을돌립니다。

金宝app仿真软件모델을시뮬레이션합니다。차량위치의실제값,측정된값,칼만필터추정값을플로팅합니다。

sim卡(“ctrlKalmanNavigationExample”);图;%绘制结果,用实线连接数据点。情节(x (: 1) x (:, 2),“软”...(: 1), y (:, 2),“gd”...xhat (: 1), xhat (:, 2),“罗”...“线型”“- - -”);标题(“位置”);包含(“东[m]”);ylabel (“北[m]”);传奇(“实际”“测量”“卡尔曼滤波器估计”“位置”“最佳”);轴

측정된위치와실제위치간의오차및칼만필터추정값과실제위치간의오차는다음과같습니다。

%东方位置测量误差[m]n_xe = y (: 1) - x (: 1);%北位置测量误差[m]n_xn = y (:, 2) - x (:, 2);%卡尔曼滤波东位置误差[m]e_xe = xhat (: 1) - x (: 1);%卡尔曼滤波北位置误差[m]e_xn = xhat (:, 2) - x (:, 2);图;%东方位置误差次要情节(2,1,1);情节(t, n_xe‘g’t e_xe“r”);ylabel ('位置误差-东[m]');包含(“时间[s]”);传奇(sprintf (“量:% .3f”规范(n_xe 1) /元素个数(n_xe)), sprintf (“卡尔曼f。:% .3f”规范(e_xe 1) /元素个数(e_xe)));轴%北位置误差次要情节(2,1,2);情节(t、y (:, 2) - x (:, 2),‘g’t xhat (:, 2) - x (:, 2),“r”);ylabel ('位置误差-北[m]');包含(“时间[s]”);传奇(sprintf (“量:% .3f”规范(n_xn 1) /元素个数(n_xn)), sprintf (“卡尔曼f: % .3f”规范(e_xn 1) /元素个数(e_xn)));轴

플롯범례는데이터점의개수로정규화된위치측정값과추정오차($ | | x_e - \帽子{x} _e | | _1美元$ | | x_n - \帽子{x} _n”| | _1美元)를표시합니다。칼만필터추정값이원시측정값보다오차가약25%적습니다。

동쪽방향의실제속도와칼만필터추정값은아래그림의위쪽플롯에표시되어있습니다。아래쪽플롯은추정오차를표시합니다。

e_ve = xhat (:, 3) - x (:, 3);% [m/s]卡尔曼滤波东速度误差e_vn = xhat (:, 4) - x (:, 4);% [m/s]卡尔曼滤波北速度误差图;%东向速度及其估计次要情节(2,1,1);情节(t) x (:, 3),“b”t xhat (:, 3),“r”);ylabel (“速度-东[m]”);包含(“时间[s]”);传奇(“实际”卡尔曼滤波器的“位置”“最佳”);轴;次要情节(2,1,2);%估计误差情节(t, e_ve“r”);ylabel (“速度误差-东[m]”);包含(“时间[s]”);传奇(sprintf (“卡尔曼滤波器:% .3f”规范(e_ve 1) /元素个数(e_ve)));轴

오차플롯의범례는데이터점의개수로정규화된동쪽속도추정오차$ | | \点{x} _e - \帽子{\点{x}} _e | | _1美元을표시합니다。

칼만필터의속도추정값이실제속도의추세를정확하게추종합니다。차량이높은속도로이동하면잡음수준이감소합니다。Q이는행렬의설계와부합합니다。t = 50年代및t = 200年代에두개의큰스파이크가있습니다。이시점은각각자동차가갑자기감속하고급히방향을돌린시간입니다。Q이순간의속도변화는행렬입력을기반으로하는칼만필터의예측값보다훨씬큽니다。몇번의시간스텝후에는필터의추정값이실제속도를따라잡습니다。

요약

金宝app仿真软件에서卡尔曼滤波器블록을사용하여차량의위치와속도를추정했습니다。이모델의공정잡음동특성은시간에따라변화했습니다。다양한차량기동과무작위로생성된측정잡음을시뮬레이션하여필터성능을확인했습니다。칼만필터를통해차량의위치측정값을향상시키고속도추정값을얻을수있었습니다。

bdclose (“ctrlKalmanNavigationExample”);

관련항목