无人机常用算法——卡尔曼滤波器（五）

sigma

无人机常用算法——卡尔曼滤波器（五） [复制链接]

2.3 卡尔曼滤波参数的估计和调整

在卡尔曼滤波的使用中，通常首先要测量噪声协方差 R。可以通过离线测量实验确定。但是确定过程噪声协方差 Q 一般更困难，因为无法直接观察过程。当对一个相对简单的过程模型加入足够的不确定性时（通过选择合适的 Q）得到的结果是可信的。无论如何，通过运行中调整 Q 和 R 可以得到较好的滤波效果。调整通常是离线的，通常借助于过程的另一个卡尔曼滤波进行系统辨识来实现。在 Q 和 R 为常数的情况下，估计误差协方差 Pk 和卡尔曼增益 Kk 都会迅速镇定，然后保持为常量。如果是这样的话，可以通过离线运行滤波器来预先确定这些参数，或者如（Grewal and Andrews 2001）所述来确定 Pk 的稳态值。

然而通常的情况是测量误差不会保持常量。例如，当观察光电跟踪器面板上的亮点时，对附近的亮点的测量误差要小于远处的。有时过程噪声 Q 在滤波器运行中是动态变化的，变成了 Qk，以调整符合不同的动态特点。例如在跟踪三维虚拟环境中的用户头部时，如果用户慢速移动，就要减小 Qk 值，如果动态变化加快就增加 Qk 值。在这类情况下，通过选择来适应用户意图的不确定性以及模型中的不确定性。

测量方程协方差的确定

测量方程是由测量设备可以实现的测量量来确定形式的，例如激光测距仪可以进行目标位置径向位置和角度位置的测量（极坐标测量），因此这两个量就是测量方程的输出。测量方程的协方差可以通过对激光测距仪的试验来确定。方法是利用在测距仪和目标都固定的情况下，激光测距仪对已知位置的目标进行多次测量，得到一组测量数据，然后利用 matlab 软件求出这组数据的协方差就可以作为测量方程的协方差了。当然测距仪在测量不同距离的目标时，测量的误差可能不同，所以就有不同的协方差，可以取一个中间位置对应的协方差为仪器的协方差。或者取测量范围内最大的协方差。

状态方程（过程方程）的协方差确定

过程方程的协方差比较难以确定，一种方法是对于比较容易观察过程变化的系统，通过经验设定协方差。另一种需要通过仿真来调整。对于给定的过程方程，先预设一个协方差，通过设定一系列标准的测量值，仿真观察卡尔曼滤波是否很快收敛，如果不收敛或者收敛慢则可以调整过程协方差来达到较好的效果。

过程、测量误差及协方差的确定

误差及协方差的准确确定比较重要，也比较困难。测量方程的误差可以通过离线测量确定，但是过程误差不容易准确估计。

设状态方程 X (k +1) = A(k)X (k) + B(k)u(k) +V (k) ，V (k) 为零均值、白色高斯过程噪声，其协方差为 Q(k) 。如果噪声V (k) 表示成 Γ(k)v(k) ， Γ(k) 为过程噪声分布矩阵，v(k) 为一个标量。则 Q(k) 变为

以目标在 x 轴方向上做匀速直线运动为例：状态向量

噪声分布矩阵为

T 为采样周期，而标量 v(k) 是0 均值，方差为 q 的量。

为何噪声分布矩阵取这样的值？因为这是过程噪声，两个过程变量分别为目标移动的位置和速度，它们的过程噪声有一定的相关性。因为设定目标为匀速运动，所以过程中产生加速度的变化引起状态变量的噪声。

这里将认为 v(k) = a ，是方差为q 的零均值变量，q 的值可以参照在加速度测量中的加速度变化情况。当然还可能存在其它噪声形式，需要通过工作过程中的系统辨识等方法来确定。

协方差的计算。 Q(k) 是噪声V (k) 的协方差。

如果噪声向量中各分量都服从各自的 0 均值正态分布：

其中：

如果噪声向量的各分量互不相关，则 E[v1 (k)v2 (k)] = 0 。

月下良缘

谢谢分享！