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

다음제품이필합니다。

스크립트열기

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

소개

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

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

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

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 \;\frac{u_T(t)}{s(t)} - A \;重金属镉\; s(t)^2) / m \\ s(t) \tan(u_\psi(t)) / L \end{array} \right] $$

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

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

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

$\begin{array} {ll} P=100000 \;& # 38;\textnormal{引擎峰值功率}\;[W] A=1 \;& # 38;\textnormal{前额区域}\;[m^2] \\ C_d=0.3 \;& # 38;\textnormal{阻力系数}\;[无单位]\\ 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}\;[无单位]\\ u_\psi(t) \;& # 38;\textnormal{转向角度}\;[deg] \\ \end{array}$

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

차량모델은ctrlKalmanNavigationExample /车辆模型서브시스템에구현되어있습니다。이S金宝appimulink모델의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} {ll} \hat{x}_e[n] \;& # 38;\textnormal{东位置估计}\;[m] \\ \hat{x}_n[n] \;& # 38;\textnormal{北方位置估计}\;[m] \\ \hat{\dot{x}}_e[n] \;& # 38;\textnormal{东速度估计}\;[m/s] \\ \hat{\dot{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{数组}{c 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{数组}{c 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]$ 입니다。

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

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

w美元 의소는차량속도가하나의샘플시간Ts에얼마나변화할수있는지를나타냅니다。공정잡음w의분산Q행렬은시변으로선택되었습니다。이것은속도가크면 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{数组}{c c} \ displaystyle 1 + \压裂{250}{f{坐在}(\帽子{\点{x}} _e ^ 2)} & # 38;0 xA \ \ & #;0 & # 38;\ displaystyle 1 + \压裂{250}{f{坐在}({\点{x}} \帽子_n”^ 2)}& # xA;结束\{数组}& # xA;\右]& # xA; $$

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

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

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

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

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

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

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

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

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

모델소스:개별a, b, c, d행렬。

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

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

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

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

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

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

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

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

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

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

N：0．공정잡음과측정잡음간에상관관계가없다고가정합니다。

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

결과

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

T = 0에서차량위치는，이고정지상태입니다。

동쪽방향으로25m/s로가속합니다。T = 50s에서5m/s로감속합니다。

T = 100秒에서북쪽으로방향을돌려20m/s로가속합니다。

T = 200s에서다시서쪽으로방향을돌립니다。25m/s로가속합니다。

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

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

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 = 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 (速度-东[米/秒]）;包含(“时间[s]”）;传奇(“实际”，卡尔曼滤波器的，“位置”，“最佳”）;轴紧；次要情节(2,1,2);估计误差%情节(t, e_ve“r”）;ylabel (速度误差-东[米/秒]）;包含(“时间[s]”）;传奇(sprintf ('卡尔曼滤波器:%.3f'规范(e_ve 1) /元素个数(e_ve)));轴紧；

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

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

요약

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

bdclose (“ctrlKalmanNavigationExample”）;