时变卡尔曼滤波状态估计

这个例子使用了:

打开脚本

这个例子展示了如何在Simulink®中使用时变卡尔曼滤波器估计线性系统的状态。金宝app的卡尔曼过滤器块系统识别工具箱/估计器库来估计地面车辆的位置和速度基于噪声位置测量，如GPS传感器测量。卡尔曼滤波器中的植物模型具有时变噪声特性。

简介

你想要估计一辆地面车辆在北面和东面的位置和速度。飞行器可以在二维空间中自由运动，不受任何约束。你设计了一个多用途的导航和跟踪系统，可以用于任何物体，而不仅仅是车辆。

美元美元x_e (t) 而且 x_n (t)美元是飞行器从原点往东和往北的位置， $\θ(t)美元$ 车辆方向是东向和 $u_ \ psi (t)美元$ 是车辆的转向角度。元新台币是连续时间变量。

Simu金宝applink模型主要由两部分组成:车辆模型和卡尔曼滤波器。这些将在以下部分中进一步解释。

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 /车辆模型子系统。Simu金宝applink模型包含两个PI控制器，用于跟踪车辆的理想方向和速度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美元而且零均值独立随机变量是否具有已知方差美元$ 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的采样率提供这些测量。测量噪声的方差五美元， R矩阵指定为 R = 50美元．由于R被指定为标量，卡尔曼滤波块假设矩阵R是对角线，它的对角线是50，并且与y的尺寸兼容。如果测量噪声是高斯分布的，R=50对应于68%的位置测量在范围内 $下午\ \ sqrt{50} \美元;m美元$ 或者是在东、北方向的实际位置。然而，这个假设对于卡尔曼滤波器是不必要的。

的要素 w美元捕获车辆速度在一个采样时间Ts内的变化量。过程噪声w的方差，即Q矩阵，被选择为时变的。它抓住了典型值的直觉 w [n]美元当速度较大时，会变小。例如，从0到10m/s比从10到20m/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 -25m/s加速时间数据进行最小二乘拟合得到的。注意，对角线Q的选择代表了一个朴素的假设，即北方向和东方向的速度变化是不相关的。

卡尔曼滤波器块输入和设置

“卡尔曼过滤器”块在系统识别工具箱/估计器Simulink中的库金宝app。它也在控制系统工具箱图书馆。配置离散状态估计的块参数。指定以下内容过滤器设置参数:

时间域:离散时间。选择此选项可估计离散时间状态。

选择利用电流测量y[n]改进xhat[n]复选框。这实现了离散时间卡尔曼滤波器的“电流估计器”变体。此选项提高了估计精度，并且对于缓慢的采样时间更有用。但是，这增加了计算成本。此外，该卡尔曼滤波器变体具有直接馈通，如果在不包含任何延迟的反馈循环中使用卡尔曼滤波器，则会导致代数循环(反馈循环本身也具有直接馈通)。代数环会进一步影响仿真速度。

单击选项选项卡设置块的输入和输出选项:

取消增加输入端口u复选框。在植物模型中没有已知的输入。

选择输出状态估计误差协方差Z复选框。Z矩阵提供了关于过滤器在状态估计中的置信度的信息。

点击模型参数指定工厂模型和噪声特性:

模型来源:单独的A B C D矩阵。

一个A矩阵在本例前面定义。

CC矩阵在本例前面定义。

初步估算来源:对话框

初始状态x[0]：0．这表示在t=0s时位置和速度估计的初始猜测为0。

状态估计误差协方差P[0]：10．假设您的初始猜测x[0]与其实际值之间的误差是一个具有标准偏差的随机变量 $\ sqrt {10}$ ．

选择使用G和H矩阵(默认G=I和H=0)复选框指定非默认G矩阵。

GG矩阵在本例前面定义。

H：0．过程噪声不影响进入卡尔曼滤波块的测量值。

取消定常问复选框。Q矩阵是时变的，并通过输入Q的块提供。由于这个设置，块使用时变卡尔曼滤波器。您可以选择此选项来使用时不变卡尔曼滤波器。时不变卡尔曼滤波器在这个问题上表现稍差，但更容易设计，计算成本更低。

R: R.这是测量噪声的协方差．在本例前面定义了R矩阵。

N：0．假设过程噪声和测量噪声之间没有相关性。

采样时间(继承的为-1): Ts，在本例前面定义了。

结果

通过模拟车辆进行以下操作的场景来测试卡尔曼滤波器的性能:

t = 0时，飞行器在，它是静止的。

向东，加速到每秒25米。在t=50s时减速到5m/s。

在t = 1000s时，转向北，加速至20m/s。

在t = 200时，它又向西转弯。它加速到25米/秒。

在t = 260s时，它减速到15m/s，并做一个180度的匀速转弯。

模拟Simulink模型。金宝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 = 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矩阵输入的。经过几个时间步后，滤波器估计赶上了实际速度。

总结

您使用Simulink中的卡尔曼滤波块估计车辆的位置和速度。金宝app模型的过程噪声动态是时变的。通过模拟各种车辆机动和随机产生的测量噪声，验证了滤波器的性能。卡尔曼滤波器改进了位置测量并为飞行器提供了速度估计。

bdclose (“ctrlKalmanNavigationExample”）;