扩招卡尔曼滤波模型java 卡尔曼滤波 实例

在上一篇卡尔曼滤波详解已经给出详细的推导过程。这里给出几个示例加深理解，这个是几年前学习卡尔曼滤波时候整理的，比较啰嗦，初学者可以看看，了解过的就不用浪费时间了，主要是最近在整理资料差点弄丢，还是放到网上保存吧。

简单示例1

这个示例中假设状态是稳定的，也就是真实的状态是不随着时间变化的，那么利用卡尔曼滤波算法最后会收敛到最优的状态。

参数设置

对于所有的变量，本场景下都是一个具体的数值而不是矩阵：

$u$

$A = 1$

$H = 1$

$\hat{x}_0 = 0$

$P_0 = 1$

$Q = 0$ 和 $R = 0.1$

基本模型
$\begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = x_{k-1} \end{aligned}$

$\begin{aligned} z_k & = Hx_k + v_k \\ & = x_k + v_k \end{aligned}$

基本公式

Time Update	Measurement Update
$\bar{\hat{x}_k} = \hat{x}_{k-1}$	$K_k = \bar{P_k}(\bar{P_k} + 0.1)^{-1}$
$\bar{P_k} = P_{k-1}$	$\hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \bar{\hat{x}_k})$
	$P_k = (1 - K_k)\bar{P_k}$

仿真实验

给定测量值：

TIME(ms)	1	2	3	4	5	6	7	8	9	10
VALUE (V)	0.39	0.50	0.48	0.29	0.25	0.32	0.34	0.48	0.41	0.45

基本的卡尔曼滤波算法的代码：

z = [0, 0.39, 0.50, 0.48, 0.29, 0.25, 0.32, 0.34, 0.48, 0.41, 0.45];
s = size(z);
xp = [0]; Pp = [1]; xpp = [0]; Ppp = [1]; K = [0];
A = 1; H = 1; Q = 0; R = 0.1;
for k=2:s(2)
    xp(k) = A*xpp(k-1);
    Pp(k) = A*Ppp(k-1)*A' + Q;
    K(k) = Pp(k)*H/(H*Pp(k)*H + R);
    xpp(k) = xp(k) + K(k)*(z(k) - H*xp(k));
    Ppp(k) = (1 - K(k)*H)*Pp(k);
end

plot(1:s(2)-1, xpp(2:s(2)), 'r', 'LineWidth', 2);
axis([1 s(2)-1 0 0.5]);

下面是卡尔曼滤波算法运行过程中基本公式变量的变化：

$k$	$z_k$	$\bar{\hat{x}_k}$	$\bar{P_k}$	$K_k$	$\hat{x}_k$	$P_k$
0					0	1
1	0.39	0	1	0.9091	0.3545	0.0909
2	0.50	0.3545	0.0909	0.4762	0.4238	0.0476
3	0.48	0.4238	0.0476	0.3226	0.4419	0.0323
4	0.29	0.4419	0.0323	0.2439	0.4049	0.0244
5	0.25	0.4049	0.0244	0.1961	0.3745	0.0196
6	0.32	0.3745	0.0196	0.1639	0.3656	0.0164
7	0.34	0.3656	0.0164	0.1408	0.3620	0.0141
8	0.48	0.3620	0.0141	0.1235	0.3765	0.0123
9	0.41	0.3765	0.0123	0.1099	0.3802	0.0110
10	0.45	0.3802	0.0110	0.0990	0.3871	0.0099

估计状态的效果图，可以看出来已经收敛了：

简单示例2

在本例子中，估计一个随机的标量常量，例如电压，真实的状态是不随着时间变化的，那么利用卡尔曼滤波算法最后会收敛到最优的状态。

参数设置

对于所有的变量，本场景下都是一个具体的数值而不是矩阵：

$u$

$A = 1$

$H = 1$

$\hat{x}_0 = 0$

$P_0 = 1$

$Q = 1e-6$

$R = 0.01$

基本模型
$\begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = x_{k-1} + w_k \end{aligned}$

$\begin{aligned} z_k & = Hx_k + v_k \\ & = x_k + v_k \end{aligned}$

基本公式

Time Update	Measurement Update
$x=-0.37727$	$x=-0.37727$
$x=-0.37727$	$x=-0.37727$
	$x=-0.37727$

仿真实验

在这里，设置要获得的标量常量 $x=-0.37727$ 。然后去模拟生成50个单独的测量数据 $z_k$

生成数据代码如下，把生成的数据保存下来，后面运行卡尔曼滤波算法时不再重新生成，直接加载：

x = -0.37727;
num = 50;
mnoise = normrnd(0, 0.1, num, 1);
z = mnoise + x;
save('z.mat', 'z');

plot(1:num, repmat(x, [1, num]), 'k', 'LineWidth', 2);
hold on;
plot(1:num, z, 'b*');
maxz = max(z);
minz = min(z);
axis([0 num+1 minz-0.01 maxz+0.01]);
xlabel('Iteration');  
ylabel('Voltage');  
set(gca,'XTick',10:10:50)  
set(gca,'YTick',-0.6:0.1:-0.1)

生成图如下，其中黑色线表示真实的状态值，而蓝色*表示添加白色高斯噪声之后的生成的测量值：

基本的卡尔曼滤波算法代码：

x = -0.37727;
num = 50;
mat = load('z.mat');
z = mat.z;

xp = [0]; Pp = [1]; xpp = [0]; Ppp = [1]; K = [0];
A = 1; H = 1; Q = 0.000001; R = 0.01;
for k=2:num+1
    xp(k) = A*xpp(k-1);
    Pp(k) = A*Ppp(k-1)*A' + Q;
    K(k) = Pp(k)*H/(H*Pp(k)*H + R);
    xpp(k) = xp(k) + K(k)*(z(k-1) - H*xp(k));
    Ppp(k) = (1 - K(k)*H)*Pp(k);
end

plot(1:num, repmat(x, [1, num]), 'k', 'LineWidth', 2);
hold on;
plot(1:num, z, 'b*');
plot(1:num, xpp(2:num+1), 'r', 'LineWidth', 2);

maxz = max(z);
minz = min(z);
axis([0 num+1 minz-0.01 maxz+0.01]);
xlabel('Iteration');  
ylabel('Voltage');  
set(gca,'XTick',10:10:50)  
set(gca,'YTick',-0.6:0.1:-0.1)  

% plot P versus iteration
figure;
plot(1:num, Ppp(2:num+1), 'k', 'LineWidth', 2);
xlabel('Iteration');  
ylabel('Pk');  
set(gca,'XTick',10:10:50)  
set(gca,'YTick',0.002:0.002:0.1)

结果如下，可以看出来虽然测量值的噪声比较大，但是利用卡尔曼滤波算法还是很快收敛到正确的值，红色线是卡尔曼滤波算法估计的值：

然后上面考虑到对于 $P_0$ 的设置问题，提到只要设置为不为0的数值即可，最后结果也会正确的收敛。在下图中是 $P_k$

前面也提到过对于 $Q$ 和 $R$

下面是把上面的 $R$

下面是把上面的 $R$

把这三个画在一张图上近距离观察一下，可以更好感受这个变化：

简单示例3

在本例子中，估计一个随机的标量变量，也就是说每个时间的状态是不一样的，例如飞机着陆时的海拔（当然也可以估计速度和燃料），那么利用卡尔曼滤波算法最后会估计每个时刻最优的状态。

参数设置

对于所有的变量，本场景下都是一个具体的数值而不是矩阵：

$u$

$A = 0.98$

$H = 1$

$\hat{x}_0 = 1000$

$P_0 = 1$

$Q = 0$

$R = 900$

基本模型
$\begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = 0.98*x_{k-1} \end{aligned}$

$\begin{aligned} z_k & = Hx_k + v_k \\ & = x_k + v_k \end{aligned}$

基本公式

Time Update	Measurement Update
$\bar{\hat{x}_k} = 0.98 * \hat{x}_{k-1}$	$K_k = \bar{P_k}(\bar{P_k} + 900)^{-1}$
$\bar{P_k} = 0.98 * P_{k-1} *0.98$	$\hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \bar{\hat{x}_k})$
	$P_k = (1 - K_k)\bar{P_k}$

仿真实验

在这里，设置最初的飞机海拔为1000 ，其他时刻的真实海拔为上一时刻海拔的0.98。然后去模拟生成100个（再多的话没意义，都是0了）单独的测量数据 $z_k$

生成数据代码如下，把生成的数据保存下来，生成具有随机性，这里结果的数据见附件altitude_z1.mat，后面运行卡尔曼滤波算法时不再重新生成，直接加载：

x0 = 1000;
num = 100;
x = [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
end
mnoise = normrnd(0, 30, num, 1);
z = mnoise + x';
save('z.mat', 'z');

plot(1:num, z, 'g', 'LineWidth', 2);
hold on;
plot(1:num, x, 'k', 'LineWidth', 2);

数据分布图如下，其中黑色线表示真实的状态值，而绿色添加白色高斯噪声之后的生成的测量值：

基本的卡尔曼滤波算法代码：

x0 = 1000;
num = 100;
x = [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
end
mat = load('z.mat');
z = mat.z;

xp = [1000]; Pp = [1]; xpp = [1000]; Ppp = [1]; K = [0];
A = 0.98; H = 1; Q = 0; R = 900;
for k=2:num+1
    xp(k) = A*xpp(k-1);
    Pp(k) = A*Ppp(k-1)*A' + Q;
    K(k) = Pp(k)*H/(H*Pp(k)*H + R);
    xpp(k) = xp(k) + K(k)*(z(k-1) - H*xp(k));
    Ppp(k) = (1 - K(k)*H)*Pp(k);
end

plot(1:num, z, 'g', 'LineWidth', 2);
hold on;
plot(1:num, x, 'k', 'LineWidth', 2);
plot(1:num, xpp(2:num+1), 'r', 'LineWidth', 2);
xlabel('Iteration');  
ylabel('Altitude');

结果如下，可以看出来虽然测量值的噪声比较大，但是利用卡尔曼滤波算法还是很快收敛到正确的值，黑色线是真实的飞机海拔，红色线是卡尔曼滤波算法估计的海拔，可以看到黑色线几乎被全部遮挡住了，说明完美拟合了：

但是为什么效果会这么好呢，好的有点不符合预期，对上面的实验修改一下：

生成成新的数据但是添加测量噪声时是在当前时刻的真实飞机海拔上添加[-100, 100]内的一个随机数，不是高斯噪声了，增加噪声来看拟合效果，其他变量条件不变：

生成数据代码如下，生成具有随机性，这里结果的数据见附件altitude_z2.mat：

x0 = 1000;
num = 100;
x = [];
z= [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
    z(i) = x(i) + randi([-100, 100], 1, 1);
end
save('z.mat', 'z');

plot(1:num, z, 'g', 'LineWidth', 2);
hold on;
plot(1:num, x, 'k', 'LineWidth', 2);

数据分布图如下，其中黑色线表示真实的状态值，而绿色添加随机数噪声之后的生成的测量值，这时候的噪声已经很大了：

那么在执行上面的卡尔曼滤波算法，里面参数不变：

看上面的结果还能拟合的这么好，在这么大的噪声前提下，这说明啥问题，现在解释一下，主要有下面两方面原因：

第一是设置了一个正确的 $x_0$

第二是设置了一个比较大的 $R$

这会导致什么情况呢？卡尔曼滤波算法基本上会忽略掉测量值的作用，因为 $R$ 太大表示测量误差太大，系统不相信该值，在示例2中也讨论过，系统会尽可能相信过程中计算的飞机海拔，同时还提供了一个完美的 $x_0$

那么好在进行卡尔曼滤波算法时设置 $x_0 = 200$，终于不是完美的拟合了吧，明显红色是另一条函数曲线，所以目前仅仅是利用了过程计算的信息，测量的值几乎忽略掉了，当然本来也是当前的测量值误差也太大：

在该基础上调整 $R = 1$ ， 可以看到下面效果要好很多了，主要是设置初始的 $x_0$

为什么说初始的 $x_0$ 会有这么大影响了，回到刚才的数据（添加高斯白噪声的），设置精确的 $R = 900$ ，然后设置 $x_0 = 200$ ，得到下面的结果，虽然说 $R$

然后和上面一样调整 $R = 1$ ， 可以看到下面效果要好很多了，主要是设置初始的 $x_0$

根据上面的测试以及示例2中的结果，如果设置的 $x_0$ 的值和真实相差太大，其实一般情况下的话，该值是根据场景自己来定义的，不会出现大的问题。但是在特殊情况下，如果该值的设置偏移比较大，那么就不能在进行卡尔曼滤波算法时设置太大的 $R$ 值，因为这种情况下，会导致测量噪声太大，系统不会相信提供的测量值，而仅仅是相信过程值，但是又因为过程值的初始值都是错误的，有很大的偏移，所以需要设置一个小的 $R$ 值，这样可以得到一个正确的估计。其实从常理上也可以推断，既然有测量值，就要尽可能的相信它，这其实是唯一的输入来源，所以 $R$ 不能设置特别大。在该示例中越小越好，可以把上面两组数据的 $R = 0.0000001$

但还有种情况就是误差特别特别大，此时也不能设置的太小，不然的话会过于相信测量数据，在示例2中已经证明了。

但是看到上面的结果，误差其实也不小了，而且提供的初值偏移很大，还能这么好的被拟合，说明卡尔曼滤波算法太强大了。

然后对这两组数据调整一下 $P_0 = 0.001$ （设置为0效果一样），其他参数 $x_0 = 200$ ，$R = 1$ ，结果如下，上面也提到该值如果太小，也会导致 $K_k$

上面也提到修改 $Q = 1$

所以根据上面对各个参数的设置调整实验，在实际使用中要尽可能设置一个合理的 $P_0$ ，然后为了更好的利用测量值，$R$ 的值不能设置太大也不能太小，尽可能合理。当然如果系统的初始量可以尽可能的设置正确是最好了，这样才能加快收敛过程。对于 $Q$ 一般来说是设置为0或者是很小的数，因为表明过程变化就是符合这个线性方程的，只是有很小的噪声扰动，设置太大，过程方程就利用不上了。而关于其他的参数 $A, B, H$

简单示例4

对示例3进行扩展，上面仅仅是估计飞机海拔，在这里估计飞机海拔(altitude)，飞机航向(heading)，飞机速度(airspeed)三个变量，然后测量仪器分别是用于测量海拔的气压计(barometer)和GPS（注意有两个测量仪器，需要融合），用于测量航向的罗盘(compass)，用于测量速度的皮托流速测定管(pitot tube)。

这种情况下会变得很复杂，基本模型中的测量方程如下：
$\left[ \begin{array} { c } { \text {barometer} _ { k } } \\ { \text {compass} _ { k } } \\ { \text {pitot} _ { k } } \\ { g p s _ { k } } \end{array} \right] = \left[ \begin{array} { c c c } { 1 } & { 0 } & { 0 } \\ { 0 } & { 1 } & { 0 } \\ { 0 } & { 0 } & { 1 } \\ { 1 } & { 0 } & { 0 } \end{array} \right] \left[ \begin{array} { c } { \text {altitude} _ { k - 1 } } \\ { \text {heading} _ { k - 1 } } \\ { \text {airspeed} _ { k - 1 } } \end{array} \right]$
本示例主要聚焦在传感器数据融合计算上，所以简化上面的模型，还是只顾及飞机海拔，但是利用气压计以及gps两个传感器的测量值来进行估计。

参数设置

对于所有的变量，本场景下都是都是以向量和矩阵的形式：

$u$

$A = 0.98$

$H = \begin{bmatrix} 1 \\ 1 \end{bmatrix}$

$\hat{x}_0 = 1000$

$P_0 = 1$

$Q = 0$

$R = \begin{bmatrix} 900 & 0 \\ 0 & 900 \end{bmatrix}$

基本模型
$\begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = 0.98 * x_{k-1} \end{aligned}$

$\begin{aligned} z_k & = Hx_k + v_k \\ & = \begin{bmatrix} 1 \\ 1 \end{bmatrix} * x_k + v_k \end{aligned}$

基本公式

Time Update	Measurement Update
$\begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = 0.98 * x_{k-1} \end{aligned}$	$\begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = 0.98 * x_{k-1} \end{aligned}$
$\begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = 0.98 * x_{k-1} \end{aligned}$	$x$
	$x$

注意其中 $x$ 和 $P$ 都是标量，$z \in R^{2 \times 1}$ 和 $K \in R^{1 \times 2}$

仿真实验

在这里，设置最初的飞机海拔为1000 ，其他时刻的真实海拔为上一时刻海拔的0.98。然后去模拟生成100个（再多的话没意义，都是0了）单独的测量数据 $z_k$

生成数据代码如下，把生成的数据保存下来，生成具有随机性，这里结果的数据见附件altitude_z.mat ，后面运行卡尔曼滤波算法时不再重新生成，直接加载：

x0 = 1000;
num = 100;
x = [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
end
bnoise = normrnd(0, 30, num, 1);
gnoise = normrnd(0, 30, num, 1);
z = [bnoise + x',gnoise + x'];
save('z.mat', 'z');

plot(1:num, z(:, 1), 'g', 'LineWidth', 2);
hold on;
plot(1:num, z(:, 2), 'm', 'LineWidth', 2);
legend('barometer', 'gps')
plot(1:num, x, 'k', 'LineWidth', 2);

数据分布图如下，其中黑色线表示真实的状态值，而绿色添加白色高斯噪声之后的生成的气压计测量值，洋红色是添加白色高斯噪声之后的生成的GPS测量值：

基本的卡尔曼滤波算法代码：

x0 = 1000;
num = 100;
x = [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
end
mat = load('z.mat');
z = mat.z;

xp = [1000]; Pp = [1]; xpp = [1000]; Ppp = [1]; K = [0 0];
A = 0.98; H = [1 1]'; Q = 0; R = [900 0;0 900];
for k=2:num+1
    xp(k) = A*xpp(k-1);
    Pp(k) = A*Ppp(k-1)*A' + Q;
    K = [K; Pp(k)*H'/(H*Pp(k)*H' + R)];
    xpp(k) = xp(k) + K(k, :)*(z(k-1, :)' - H*xp(k));
    Ppp(k) = (1 - K(k, :)*H)*Pp(k);
end

plot(1:num, z(:, 1), 'g', 'LineWidth', 2);
hold on;
plot(1:num, z(:, 2), 'm', 'LineWidth', 2);
legend('barometer', 'gps')
plot(1:num, x, 'k', 'LineWidth', 2);
plot(1:num, xpp(2:num+1), 'r', 'LineWidth', 2);
xlabel('Iteration');  
ylabel('Altitude');

结果如下，可以看出来虽然测量值的噪声比较大，但是利用卡尔曼滤波算法还是很快收敛到正确的值，黑色线是真实的飞机海拔，红色线是卡尔曼滤波算法估计的海拔，可以看到黑色线几乎被全部遮挡住了，说明完美拟合了：

和示例3一样设置 $x_0 = 200$ ，然后随后设置 $R=1, 0.0000001$

简单示例5

再给出一个简单的小栗子，大家很熟悉的自由落体运动，在这里测量值只是针对高度。

参数设置

对于所有的变量，本场景下都是都是以向量和矩阵的形式：

$u$

$x$

$A$ 和 $B$

$H = \begin{bmatrix} 1 & 0 \end{bmatrix}$

$\hat{x}_0 = \begin{bmatrix} 100 \\ 0 \end{bmatrix}$

$P_0 = \begin{bmatrix} 1 & 1 \\ 1 & 1 \end{bmatrix}$

$Q = 0$

$R = 1$

基本模型

过程模型，包括高度（这里表示的是下降的高度，所以当前高度等于上一高度加上该时间段内上升的高度，在这里是下降，所以速度和加速度都是负值）以及速度：
$y_k = y_{k-1} + v_{k-1}\Delta{t} + \frac{1}{2}a(\Delta{t})^2 \\ v_k = v_{k-1} + a\Delta{t}$
所以把这两个状态值统一起来，（这里的 $v$ 表示速度不是测量误差）：
$\begin{aligned} x_k & = \begin{bmatrix} y_k \\ v_k\end{bmatrix} \\ & = \begin{bmatrix} 1 & \Delta{t} \\ 0 & 1 \end{bmatrix}x_{k-1} + \begin{bmatrix} \frac{\Delta{t}^2}{2} \\ \Delta{t} \end{bmatrix}a \end{aligned}$
而且在这里 $\Delta{t} = 1$ 和 $a = -9.81$ ，所以上面的 $A = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}$ 和 $B = \begin{bmatrix} \frac{1}{2} \\ 1 \end{bmatrix}$

所以得到最终的模型：
$x_k = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}x_{k-1} + \begin{bmatrix} \frac{1}{2} \\ 1 \end{bmatrix}* -9.81$

$\begin{aligned} z_k & = Hx_k + v_k \\ & = \begin{bmatrix} 1 & 0 \end{bmatrix} * x_k + v_k \end{aligned}$

基本公式

Time Update	Measurement Update
$\bar{\hat{x}_k} = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}\hat{x}_{k-1} + \begin{bmatrix} \frac{1}{2} \\ 1 \end{bmatrix}* -9.81$	$K_k = \bar{P_k}\begin{bmatrix} 1 \\ 0 \end{bmatrix}(\begin{bmatrix} 1 & 0 \end{bmatrix}\bar{P_k}\begin{bmatrix} 1 \\ 0 \end{bmatrix} + 1)^{-1}$
$\bar{P_k} = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}P_{k-1}\begin{bmatrix} 1 & 0 \\ 1 & 1 \end{bmatrix}$	$\hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \begin{bmatrix} 1 & 0 \end{bmatrix}\bar{\hat{x}_k})$
	$P_k = (I - K_k\begin{bmatrix} 1 & 0 \end{bmatrix})\bar{P_k}$

注意其中 $x \in R^{2 \times 1}$ ，$P \in R^{2 \times 2}$ ，$K \in R^{2 \times 1}$

仿真实验

在这里，给出6组测量数据，如下：

TIME(ms)	1	2	3	4	5	6
HEIGHT(V)	127.0	115.3	110.9	72.4	50.7	0.3

基本的卡尔曼滤波算法的代码：

num = 6;
z = [ 127.0, 115.3, 110.9, 72.4, 50.7, 0.3];
x = [125];
xp = [100 0]'; Pp{1} = [1 1;1 1]; xpp = [100 0]'; Ppp{1} = [1 1;1 1]; K = [0 0]';
A = [1 1;0 1]; B = [1/2, 1]'; u = -9.81; H = [1 0]; Q = 0; R = 10;
for k=2:num
    x = [x x(1)-1/2*9.81*(k-1)^2];
    xp = [xp A*xpp(:, k-1) + B*u];
    Pp{k} = A*Ppp{k-1}*A' + Q;
    K = [K Pp{k}*H'/(H*Pp{k}*H' + R)];
    xpp = [xpp xp(:, k) + K(:, k)*(z(k) - H*xp(:, k))];
    Ppp{k} = (1 - K(:, k)*H)*Pp{k};
end

plot(0:num-1, z, 'g', 'LineWidth', 2);
hold on;
plot(0:num-1, x(1:num), 'k', 'LineWidth', 2);
plot(0:num-1, xpp(1,1:num), 'r', 'LineWidth', 2);
xlabel('height');  
ylabel('time');

这个例子和之前的比较类似，就不详细介绍了。