基于时变卡尔曼滤波器的状态估计
这个例子展示了如何在Simulink®中使用时变卡尔曼滤波器估计线性系统的状态。您可以使用来自控制系统工具箱™库的卡尔曼滤波块,基于噪声位置测量(如GPS传感器测量)来估计地面车辆的位置和速度。卡尔曼滤波中的植物模型具有时变噪声特性。
简介
你要估计地面车辆在北面和东面的位置和速度。飞行器可以在二维空间中自由移动,不受任何限制。你设计了一个多用途的导航和跟踪系统,可以用于任何物体,而不仅仅是车辆。
而且是飞行器的东面和北面的位置,车辆的方向是东向还是东向是车辆的转向角度。是连续时间变量。
Simulink模型主要由两部分组成:车辆模型和卡尔曼滤波。下面几节将对此进行进一步解释。
open_system (“ctrlKalmanNavigationExample”);
汽车模型
履带车辆用简单的点质量模型表示:
车辆状态如下:
车辆参数为:
控制输入为:
模型的纵向动力学忽略了轮胎滚动阻力。该模型的横向动力学假设所需的转向角可以瞬时实现,忽略偏航惯性矩。
汽车模型实现在ctrlKalmanNavigationExample /车辆模型
子系统。Simulink模型包含两个PI控制器,用于跟踪汽车的所需方向和速度ctrlKalmanNavigationExample/速度和方向跟踪
子系统。这允许您为汽车指定各种操作条件,并测试卡尔曼滤波器的性能。
卡尔曼滤波器设计
卡尔曼滤波是一种基于线性模型估计感兴趣的未知变量的算法。这个线性模型描述了在响应模型初始条件以及已知和未知模型输入时,估计变量随时间的变化。在本例中,您估计了以下参数/变量:
在哪里
的术语表示速度,而不是导数算子。是离散时间索引。卡尔曼滤波中使用的模型形式为:
在哪里是状态向量,的测量,是过程噪音,和是测量噪声。卡尔曼滤波假设而且是零均值独立随机变量,方差已知吗,,.这里,A, G, C矩阵是:
在哪里
A和G的第三行将向东的速度建模为随机游走:.在现实中,位置是一个连续时间变量,是速度除以时间的积分.A和G的第一行表示这个运动学关系的离散近似:.第二行和第四行的A和G代表了相同的北速度和位置的关系。
C矩阵表示只有位置测量是可用的。位置传感器,如GPS,以1Hz的采样率提供这些测量。测量噪声的方差, R矩阵指定为.由于R被指定为标量,卡尔曼滤波块假设矩阵R是对角线,它的对角线为50,并且与y具有兼容的维度。如果测量噪声为高斯,R=50对应于68%的位置测量在或者是东、北方向的实际位置。然而,这种假设对于卡尔曼滤波器是不必要的。
的元素捕获车辆速度在一个采样时间Ts内的变化量。过程噪声w的方差,即Q矩阵,被选择为时变的。它抓住了典型值的直觉当速度大的时候会变小。例如,从0到10m/s比从10到20m/s更容易。具体来说,你使用估计的北和东速度和一个饱和函数来构造Q[n]:
Q的对角线表示w的方差与估计速度的平方成反比。饱和函数防止Q变得太大或太小。对一般车辆的0-5、5-10、10-15、15-20、20-25m/s加速时间数据进行最小二乘拟合得到系数250。注意对角线Q的选择代表了一个幼稚的假设,即北方向和东方向的速度变化是不相关的。
卡尔曼滤波器块输入和设置
“卡尔曼滤波器”块在控制系统工具箱
图书馆在仿真软件。它也在系统辨识工具箱/估计
图书馆。配置离散时间状态估计的块参数。指定以下过滤器设置参数:
时间域:离散时间。选择此选项可估计离散时间状态。
选择使用电流测量y[n]改进xhat[n]复选框。这实现了离散时间卡尔曼滤波器的“电流估计器”变体。这个选项提高了估计精度,对于较慢的采样时间更有用。然而,它增加了计算成本。此外,这种卡尔曼滤波器变体具有直接馈通,如果在不包含任何延迟的反馈循环中使用卡尔曼滤波器(反馈循环本身也具有直接馈通),则会导致代数循环。代数环路会进一步影响仿真速度。
单击选项选项卡设置块的导入和输出选项:
取消添加输入端口u复选框。在植物模型中没有已知的输入。
选择输出状态估计误差协方差Z复选框。Z矩阵提供了关于滤波器在状态估计中的置信度的信息。
点击模型参数指定工厂模型和噪声特性:
模型来源:单独的A B C D矩阵。
一个: A. A矩阵在本例前面已定义。
CC矩阵在本例前面已经定义。
初步估计源:对话框
初始状态x [0]:
0
.这表示在t=0时的位置和速度估计的初始猜测为0。
状态估计误差协方差P[0]:
10
.假设最初的猜测x[0]与实际值之间的误差是一个具有标准偏差的随机变量.
选择使用G和H矩阵(默认G=I和H=0)复选框指定一个非默认G矩阵。
GG矩阵在本例前面已经定义。
H:
0
.过程噪声不影响进入卡尔曼滤波块的测量值。
取消定常问复选框。Q矩阵是时变的,并通过块输入Q提供。由于这个设置,块使用时变卡尔曼滤波器。您可以选择此选项来使用时不变卡尔曼滤波器。时不变卡尔曼滤波器在这个问题上的表现略差,但更容易设计,计算成本更低。
R: r。这是测量噪声的协方差.R矩阵在前面的例子中已经定义了。
N:
0
.假设过程噪声和测量噪声之间没有相关性。
采样时间(继承的为-1): Ts,该值在本例前面定义。
结果
通过模拟车辆进行以下机动的场景来测试卡尔曼滤波器的性能:
当t = 0时,飞行器在,而且是静止的。
向东行驶,加速到每秒25米。当t=50时,它减速到5m/s。
当t = 100s时,它转向北方,加速到20m/s。
在t = 200时,它又向西转向。它加速到每秒25米。
在t = 260s时,它减速到15m/s,并做一个恒速180度转弯。
模拟Simulink模型。绘制车辆位置的实际,测量和卡尔曼滤波估计。
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)));轴紧;
图示显示位置测量和估计误差(而且)由数据点的数量归一化。卡尔曼滤波估计的误差比原始测量值少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)));轴紧;
误差图上的图例显示了东方速度估计误差由数据点的数量归一化。
卡尔曼滤波速度估计正确地跟踪实际速度趋势。当车辆高速行驶时,噪音水平降低。这符合Q矩阵的设计。最大的两个峰值是在t=50s和t=200s处。这是汽车分别经历突然减速和急转弯的时候。这些瞬间的速度变化比卡尔曼滤波器的预测要大得多,卡尔曼滤波器是基于它的Q矩阵输入的。经过几个时间步后,滤波器估计会赶上实际速度。
总结
您使用Simulink中的卡尔曼滤波块估计了车辆的位置和速度。模型的过程噪声动态是时变的。你们通过模拟各种车辆机动和随机产生的测量噪声来验证滤波器的性能。卡尔曼滤波器改进了位置测量,并为飞行器提供了速度估计。
bdclose (“ctrlKalmanNavigationExample”);