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

sigma

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

2.4 卡尔曼滤波初始化

初始化也比较重要，初始化偏差大容易振动，甚至可能不收敛（？）。

例如跟踪一辆车，状态空间为

如果知道足够精确的车的初始位置，那么可以初始化

或者别的具体值。并且，通过给状态协方差矩阵赋较小的值来告诉滤波器我们知道确切的初始位置，例如初始化为

或者给定其它较小的值。

如果我们不确切的知道最初的位置与速度，那么协方差矩阵可以初始化为一个对角线元素是 b 的矩阵，b取一个合适的比较大的数。

此时，滤波器更倾向于使用初次测量值的信息。

1、二维状态向量估计

设雷达进行一维跟踪时，状态方程为

测量噪声V(k) ~ N(0,r)（即 V 符合均值为 0，方差为 r 的正态分布），且与过程噪声独立。这时状态估计初始化可以采用初始两点差分，利用第一和第二时刻的两个测量值 Z(0)和 Z(1)进行初始化。

初始协方差为：

T 为测量的采样周期。状态估计和滤波从 k＝2 时刻开始。

2 四维状态向量估计的初始化

属于两坐标雷达的数据处理，系统的状态向量表示为

状态方程为：X(k) = AX(k−1)+W(k−1)，测量方程为：Z(k)= CX(k) +V(k)，测量状态向量：

                                       （a）

注意这里选择直角坐标系表示目标状态数据，但是雷达的测量数据是以极坐标形式表示的，这里在测量变量里把极坐标数据转化为直角坐标数据，这样做的目的或许是把测量方程也变成了线性方程，这样的话，就可以用普通的卡尔曼滤波方法。否则的话，测量向量以极坐标表示，写成

            （b）

是非线性方程，只能写成 Z(k)=f(X(k))+V(k)的形式，需要用扩展卡尔曼滤波方法。

系统的初始状态可利用前两个时刻的测量值 z(0)和 z(1)来确定，即

                                          （c）

由于初始状态值是由测量值给定的，所以初始状态协方差矩阵 P 也是由测量方差相应地设定。由于测量方程中的测量量（即输出量）是由真实测量值ρ和θ转化来的，所以测量 Z 的测量噪声也需要由直接测量值ρ和θ的测量噪声求出。

k 时刻测量噪声在直角坐标系下的协方差为：

                       （d）

当然如果测量方程的输出直接就是 ρ 和θ ，测量向量是式（b）的形式，而状态方程不变，那么初始的状态协方差阵可能仍然要写成（f）的形式，因为状态量仍然是从测量量转化过来的，它们之间的关系没有变。

但是如果状态方程发生改变，直接以 ρ 和θ 以及它们的速度为状态向量，测量方程也是以 ρ 和θ 输出：

状态方程为：X (k) = AX (k−1) +W (k−1) ，测量方程为：Z(k) = CX (k) +V (k) 测量噪声 V（k）为均值为零，协方差为 R（k）。

则

月下良缘

谢谢分享！