卡尔曼滤波示例

最新推荐文章于 2024-07-31 16:49:23 发布
最新推荐文章于 2024-07-31 16:49:23 发布
阅读量8.4k 收藏 173
点赞数 44
分类专栏： 机器人 文章标签： 卡尔曼滤波示例 卡尔曼滤波 kalman filter
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/honyniu/article/details/88698442
版权

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

简单示例1

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

参数设置

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

u u u 是被忽略的；

A = 1 A = 1 A=1 因为已经知道在该场景下，上一个状态和当前状态应该是相同的；

H = 1 H = 1 H=1 一般来说测量值都是状态加上一些噪声，测量和要估计的状态是一致的，在实际应用都一般都是1；

x ^ 0 = 0 \hat{x}_0 = 0 x^0​=0 设置最初的状态为0，肯定不是真实的状态，但最后系统会收敛到真实状态；

P 0 = 1 P_0 = 1 P0​=1 上面提到过不能设置为0，可以试一下，最后所有估计的状态都是0了；

Q = 0 Q = 0 Q=0 和 R = 0.1 R = 0.1 R=0.1 。

基本模型
x k = A x k − 1 + B u k − 1 + w k = x k − 1 \begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = x_{k-1} \end{aligned} xk​​=Axk−1​+Buk−1​+wk​=xk−1​​

z k = H x k + v k = x k + v k \begin{aligned} z_k & = Hx_k + v_k \\ & = x_k + v_k \end{aligned} zk​​=Hxk​+vk​=xk​+vk​​

基本公式

Time UpdateMeasurement Update
x ^ k ˉ = x ^ k − 1 \bar{\hat{x}_k} = \hat{x}_{k-1} x^k​ˉ​=x^k−1​ K k = P k ˉ ( P k ˉ + 0.1 ) − 1 K_k = \bar{P_k}(\bar{P_k} + 0.1)^{-1} Kk​=Pk​ˉ​(Pk​ˉ​+0.1)−1
P k ˉ = P k − 1 \bar{P_k} = P_{k-1} Pk​ˉ​=Pk−1​ x ^ k = x ^ k ˉ + K k ( z k − x ^ k ˉ ) \hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \bar{\hat{x}_k}) x^k​=x^k​ˉ​+Kk​(zk​−x^k​ˉ​)
P k = ( 1 − K k ) P k ˉ P_k = (1 - K_k)\bar{P_k} Pk​=(1−Kk​)Pk​ˉ​

仿真实验

给定测量值：

TIME(ms)12345678910
VALUE (V)0.390.500.480.290.250.320.340.480.410.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 k k z k z_k zk​ x ^ k ˉ \bar{\hat{x}_k} x^k​ˉ​ P k ˉ \bar{P_k} Pk​ˉ​ K k K_k Kk​ x ^ k \hat{x}_k x^k​ P k P_k Pk​
001
10.39010.90910.35450.0909
20.500.35450.09090.47620.42380.0476
30.480.42380.04760.32260.44190.0323
40.290.44190.03230.24390.40490.0244
50.250.40490.02440.19610.37450.0196
60.320.37450.01960.16390.36560.0164
70.340.36560.01640.14080.36200.0141
80.480.36200.01410.12350.37650.0123
90.410.37650.01230.10990.38020.0110
100.450.38020.01100.09900.38710.0099

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

example result2

简单示例2

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

参数设置

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

u u u 是被忽略的；

A = 1 A = 1 A=1 因为已经知道在该场景下， 上一时刻电压和当前电压应该是相同的（不考虑噪声的话）；

H = 1 H = 1 H=1 电压表测量的值和要估计的值是相同的（不算噪声的话）；

x ^ 0 = 0 \hat{x}_0 = 0 x^0​=0 设置最初的状态为0，肯定不是真实的状态，但最后系统会收敛到真实状态；

P 0 = 1 P_0 = 1 P0​=1 上面提到过不能设置为0，可以试一下，最后所有估计的状态都是0了，只要设置不为0结果就可以正确收敛；

Q = 1 e − 6 Q = 1e-6 Q=1e−6 和在这里也可以设置为0，但设置一个非0小数值可以更灵活的来调整滤波，其实在该示例中生成的数据是没有过程噪声的；

R = 0.01 R = 0.01 R=0.01 电压表模拟信号转化为数字信号不是完全精确有误差，假设有0.1的RMS白测量噪声，也就是标准差是0.1，那么方差为0.01。

基本模型
x k = A x k − 1 + B u k − 1 + w k = x k − 1 + w k \begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = x_{k-1} + w_k \end{aligned} xk​​=Axk−1​+Buk−1​+wk​=xk−1​+wk​​

z k = H x k + v k = x k + v k \begin{aligned} z_k & = Hx_k + v_k \\ & = x_k + v_k \end{aligned} zk​​=Hxk​+vk​=xk​+vk​​

基本公式

Time UpdateMeasurement Update
x ^ k ˉ = x ^ k − 1 \bar{\hat{x}_k} = \hat{x}_{k-1} x^k​ˉ​=x^k−1​ K k = P k ˉ ( P k ˉ + 0.01 ) − 1 K_k = \bar{P_k}(\bar{P_k} + 0.01)^{-1} Kk​=Pk​ˉ​(Pk​ˉ​+0.01)−1
P k ˉ = P k − 1 + ( 1 e − 6 ) \bar{P_k} = P_{k-1} + (1e-6) Pk​ˉ​=Pk−1​+(1e−6) x ^ k = x ^ k ˉ + K k ( z k − x ^ k ˉ ) \hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \bar{\hat{x}_k}) x^k​=x^k​ˉ​+Kk​(zk​−x^k​ˉ​)
P k = ( 1 − K k ) P k ˉ P_k = (1 - K_k)\bar{P_k} Pk​=(1−Kk​)Pk​ˉ​

仿真实验

在这里，设置要获得的标量常量 x = − 0.37727 x=-0.37727 x=−0.37727 。然后去模拟生成50个单独的测量数据 z k z_k zk​ ，对每一个测量值随机生成测量噪声（服从均值为0，标准差为0.1的高斯分布）添加到上面设置的常量上生成测量值。同时可能需要做多组实验，预先生成测量集合可以保持每组实验数据是完全相同的。

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

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)

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

example result3

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

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)

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

example result4

然后上面考虑到对于 P 0 P_0 P0​ 的设置问题，提到只要设置为不为0的数值即可，最后结果也会正确的收敛。在下图中是 P k P_k Pk​ 的变化曲线，可以看出最后变为0.0002。

example result5

前面也提到过对于 Q Q Q 和 R R R 取值来说不是特别重要，最后结果都会收敛，但也说到，如果对这些噪声估计的更好，能得到更好的结果。

下面是把上面的 R R R 设置为1得到的结果，该值变大了，说明测量误差较大，在估计过程中不会太相信这个测量值，所以最开始从初始值向下的波动，而没有太大的向测量值的波动：

example result6

下面是把上面的 R R R 设置为0.0001得到的结果，该值变小了，说明测量误差较小，在估计过程中就会尽可能去相信这个测量值（包含噪声的测量值），所以会有偏向测量值的波动：

example result7

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

example result8

简单示例3

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

参数设置

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

u u u 是被忽略的；

A = 0.98 A = 0.98 A=0.98 在该场景下，假设每个时刻飞机海拔下降2%，所以当前时刻飞机海拔是上一时刻飞机海拔的0.98（不考虑噪声的话）；

H = 1 H = 1 H=1 GPS或气压计等传感器获得的海拔和真实海拔是一致的（不算噪声的话）；

x ^ 0 = 1000 \hat{x}_0 = 1000 x^0​=1000 设置最初的状态为1000，最初的飞机海拔；

P 0 = 1 P_0 = 1 P0​=1 上面提到过不能设置为0，可以试一下，最后所有估计的状态都是0了，只要设置不为0结果就可以正确收敛；

Q = 0 Q = 0 Q=0 在该示例中生成的数据是没有过程噪声的；

R = 900 R = 900 R=900 测量海拔的传感器提供不同程度的精度，标准差是30，那么方差为900，其实不用这是这么高，主要是防止噪声不明显看不出来效果。

基本模型
x k = A x k − 1 + B u k − 1 + w k = 0.98 ∗ x k − 1 \begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = 0.98*x_{k-1} \end{aligned} xk​​=Axk−1​+Buk−1​+wk​=0.98∗xk−1​​

z k = H x k + v k = x k + v k \begin{aligned} z_k & = Hx_k + v_k \\ & = x_k + v_k \end{aligned} zk​​=Hxk​+vk​=xk​+vk​​

基本公式

Time UpdateMeasurement Update
x ^ k ˉ = 0.98 ∗ x ^ k − 1 \bar{\hat{x}_k} = 0.98 * \hat{x}_{k-1} x^k​ˉ​=0.98∗x^k−1​ K k = P k ˉ ( P k ˉ + 900 ) − 1 K_k = \bar{P_k}(\bar{P_k} + 900)^{-1} Kk​=Pk​ˉ​(Pk​ˉ​+900)−1
P k ˉ = 0.98 ∗ P k − 1 ∗ 0.98 \bar{P_k} = 0.98 * P_{k-1} *0.98 Pk​ˉ​=0.98∗Pk−1​∗0.98 x ^ k = x ^ k ˉ + K k ( z k − x ^ k ˉ ) \hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \bar{\hat{x}_k}) x^k​=x^k​ˉ​+Kk​(zk​−x^k​ˉ​)
P k = ( 1 − K k ) P k ˉ P_k = (1 - K_k)\bar{P_k} Pk​=(1−Kk​)Pk​ˉ​

仿真实验

在这里，设置最初的飞机海拔为1000 ，其他时刻的真实海拔为上一时刻海拔的0.98。然后去模拟生成100个（再多的话没意义，都是0了）单独的测量数据 z k z_k zk​ ，对每一个测量值随机生成测量噪声添加到该时刻的真实海拔上生成测量值。同时可能需要做多组实验，预先生成测量集合可以保持每组实验数据是完全相同的。

生成数据代码如下，把生成的数据保存下来，生成具有随机性，这里结果的数据见附件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);

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

example result9

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

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');

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

example result10

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

生成成新的数据但是添加测量噪声时是在当前时刻的真实飞机海拔上添加[-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);

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

example result11

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

example result12

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

第一是设置了一个正确的 x 0 x_0 x0​ ；

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

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

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

example result13

在该基础上调整 R = 1 R = 1 R=1 ， 可以看到下面效果要好很多了，主要是设置初始的 x 0 x_0 x0​ 差距太大，所以该示例也给以后使用提供了经验：

example result14

为什么说初始的 x 0 x_0 x0​ 会有这么大影响了，回到刚才的数据（添加高斯白噪声的），设置精确的 R = 900 R = 900 R=900 ，然后设置 x 0 = 200 x_0 = 200 x0​=200 ，得到下面的结果，虽然说 R R R 设置是非常精确的，但是结果却不理想：

example result15

然后和上面一样调整 R = 1 R = 1 R=1 ， 可以看到下面效果要好很多了，主要是设置初始的 x 0 x_0 x0​ 差距太大，所以该示例也给以后使用提供了经验：

example result16

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

example result17 example result18

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

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

然后对这两组数据调整一下 P 0 = 0.001 P_0 = 0.001 P0​=0.001 （设置为0效果一样），其他参数 x 0 = 200 x_0 = 200 x0​=200 ， R = 1 R = 1 R=1 ，结果如下，上面也提到该值如果太小，也会导致 K k K_k Kk​ 的值太小，所以在估计过程中不会去相信测量值：

example result19 example result20

上面也提到修改 Q = 1 Q = 1 Q=1 （设置太小都不会起作用） 可能会改变结果，但是手动的引入过程噪声，说明不能相信过程处理，那么在这种情况下，系统会极大的相信测量值，波动太大，如下：

example result21 example result22

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

简单示例4

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

这种情况下会变得很复杂，基本模型中的测量方程如下：
[ barometer k compass k pitot k g p s k ] = [ 1 0 0 0 1 0 0 0 1 1 0 0 ] [ altitude k − 1 heading k − 1 airspeed k − 1 ] \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] ⎣⎢⎢⎡​barometerk​compassk​pitotk​gpsk​​⎦⎥⎥⎤​=⎣⎢⎢⎡​1001​0100​0010​⎦⎥⎥⎤​⎣⎡​altitudek−1​headingk−1​airspeedk−1​​⎦⎤​
本示例主要聚焦在传感器数据融合计算上，所以简化上面的模型，还是只顾及飞机海拔，但是利用气压计以及gps两个传感器的测量值来进行估计。

参数设置

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

u u u 是被忽略的；

A = 0.98 A = 0.98 A=0.98 在该场景下，假设每个时刻飞机海拔下降2%，所以当前时刻飞机海拔是上一时刻飞机海拔的0.98（不考虑噪声的话）；

H = [ 1 1 ] H = \begin{bmatrix} 1 \\ 1 \end{bmatrix} H=[11​] 多个传感器融合；

x ^ 0 = 1000 \hat{x}_0 = 1000 x^0​=1000 设置最初的状态为1000，最初的飞机海拔；

P 0 = 1 P_0 = 1 P0​=1 上面提到过不能设置为0，可以试一下，最后所有估计的状态都是0了，只要设置不为0结果就可以正确收敛；

Q = 0 Q = 0 Q=0 在该示例中生成的数据是没有过程噪声的；

R = [ 900 0 0 900 ] R = \begin{bmatrix} 900 & 0 \\ 0 & 900 \end{bmatrix} R=[9000​0900​] 测量海拔的传感器提供不同程度的精度，标准差是30，那么方差为900，气压计和GPS的噪声分布相同，但是独立分布的。

基本模型
x k = A x k − 1 + B u k − 1 + w k = 0.98 ∗ x k − 1 \begin{aligned} x_k & = Ax_{k-1} + Bu_{k-1} + w_k \\ & = 0.98 * x_{k-1} \end{aligned} xk​​=Axk−1​+Buk−1​+wk​=0.98∗xk−1​​

z k = H x k + v k = [ 1 1 ] ∗ x k + v k \begin{aligned} z_k & = Hx_k + v_k \\ & = \begin{bmatrix} 1 \\ 1 \end{bmatrix} * x_k + v_k \end{aligned} zk​​=Hxk​+vk​=[11​]∗xk​+vk​​

基本公式

Time UpdateMeasurement Update
x ^ k ˉ = 0.98 ∗ x ^ k − 1 \bar{\hat{x}_k} = 0.98 * \hat{x}_{k-1} x^k​ˉ​=0.98∗x^k−1​ K k = P k ˉ [ 1 1 ] ( [ 1 1 ] P k ˉ [ 1 1 ] + [ 900 0 0 900 ] ) − 1 K_k = \bar{P_k}\begin{bmatrix} 1 & 1 \end{bmatrix}(\begin{bmatrix} 1 \\ 1 \end{bmatrix}\bar{P_k}\begin{bmatrix} 1 & 1 \end{bmatrix} + \begin{bmatrix} 900 & 0 \\ 0 & 900 \end{bmatrix})^{-1} Kk​=Pk​ˉ​[1​1​]([11​]Pk​ˉ​[1​1​]+[9000​0900​])−1
P k ˉ = 0.98 ∗ P k − 1 ∗ 0.98 \bar{P_k} = 0.98 * P_{k-1} * 0.98 Pk​ˉ​=0.98∗Pk−1​∗0.98 x ^ k = x ^ k ˉ + K k ( z k − [ 1 1 ] x ^ k ˉ ) \hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \begin{bmatrix} 1 \\ 1 \end{bmatrix}\bar{\hat{x}_k}) x^k​=x^k​ˉ​+Kk​(zk​−[11​]x^k​ˉ​)
P k = ( 1 − K k [ 1 1 ] ) P k ˉ P_k = (1 - K_k\begin{bmatrix} 1 \\ 1 \end{bmatrix})\bar{P_k} Pk​=(1−Kk​[11​])Pk​ˉ​

注意其中 x x x 和 P P P 都是标量， z ∈ R 2 × 1 z \in R^{2 \times 1} z∈R2×1 和 K ∈ R 1 × 2 K \in R^{1 \times 2} K∈R1×2 。

仿真实验

在这里，设置最初的飞机海拔为1000 ，其他时刻的真实海拔为上一时刻海拔的0.98。然后去模拟生成100个（再多的话没意义，都是0了）单独的测量数据 z k z_k zk​ ，对每一个测量值随机生成测量噪声添加到该时刻的真实海拔上生成测量值，当然是对气压计和GPS单独模拟生成，同时可能需要做多组实验，预先生成测量集合可以保持每组实验数据是完全相同的。

生成数据代码如下，把生成的数据保存下来，生成具有随机性，这里结果的数据见附件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测量值：

example result23

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

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');

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

example result24

和示例3一样设置 x 0 = 200 x_0 = 200 x0​=200 ，然后随后设置 R = 1 , 0.0000001 R=1, 0.0000001 R=1,0.0000001 得到下面三个结果，和示例3结论一致：

example result25 example result26 example result27

简单示例5

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

参数设置

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

u u u 该部分是加速度部分，下面给出模型的时候会给出具体值；

x x x 在该场景下表示需要估计物体的高度以及当前速度；

A A A 和 B B B 在下面后给出具体的值；

H = [ 1 0 ] H = \begin{bmatrix} 1 & 0 \end{bmatrix} H=[1​0​] 在该场景下需要估计物体高度以及速度，但测量值只有高度，所以忽略速度项；

x ^ 0 = [ 100 0 ] \hat{x}_0 = \begin{bmatrix} 100 \\ 0 \end{bmatrix} x^0​=[1000​] 设置最初的物体的高度为100，速度为0；

P 0 = [ 1 1 1 1 ] P_0 = \begin{bmatrix} 1 & 1 \\ 1 & 1 \end{bmatrix} P0​=[11​11​] 上面提到过不能设置为0，可以试一下，最后所有估计的状态都是0了，只要设置不为0结果就可以正确收敛；

Q = 0 Q = 0 Q=0 在该示例中生成的数据是没有过程噪声的；

R = 1 R = 1 R=1 测量高度误差项协方差。

基本模型

过程模型，包括高度（这里表示的是下降的高度，所以当前高度等于上一高度加上该时间段内上升的高度，在这里是下降，所以速度和加速度都是负值）以及速度：
y k = y k − 1 + v k − 1 Δ t + 1 2 a ( Δ t ) 2 v k = v k − 1 + a Δ t 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} yk​=yk−1​+vk−1​Δt+21​a(Δt)2vk​=vk−1​+aΔt
所以把这两个状态值统一起来，（这里的 v v v 表示速度不是测量误差）：
x k = [ y k v k ] = [ 1 Δ t 0 1 ] x k − 1 + [ Δ t 2 2 Δ t ] a \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} xk​​=[yk​vk​​]=[10​Δt1​]xk−1​+[2Δt2​Δt​]a​
而且在这里 Δ t = 1 \Delta{t} = 1 Δt=1 和 a = − 9.81 a = -9.81 a=−9.81 ，所以上面的 A = [ 1 1 0 1 ] A = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix} A=[10​11​] 和 B = [ 1 2 1 ] B = \begin{bmatrix} \frac{1}{2} \\ 1 \end{bmatrix} B=[21​1​]

所以得到最终的模型：
x k = [ 1 1 0 1 ] x k − 1 + [ 1 2 1 ] ∗ − 9.81 x_k = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}x_{k-1} + \begin{bmatrix} \frac{1}{2} \\ 1 \end{bmatrix}* -9.81 xk​=[10​11​]xk−1​+[21​1​]∗−9.81

z k = H x k + v k = [ 1 0 ] ∗ x k + v k \begin{aligned} z_k & = Hx_k + v_k \\ & = \begin{bmatrix} 1 & 0 \end{bmatrix} * x_k + v_k \end{aligned} zk​​=Hxk​+vk​=[1​0​]∗xk​+vk​​

基本公式

Time UpdateMeasurement Update
x ^ k ˉ = [ 1 1 0 1 ] x ^ k − 1 + [ 1 2 1 ] ∗ − 9.81 \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 x^k​ˉ​=[10​11​]x^k−1​+[21​1​]∗−9.81 K k = P k ˉ [ 1 0 ] ( [ 1 0 ] P k ˉ [ 1 0 ] + 1 ) − 1 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} Kk​=Pk​ˉ​[10​]([1​0​]Pk​ˉ​[10​]+1)−1
P k ˉ = [ 1 1 0 1 ] P k − 1 [ 1 0 1 1 ] \bar{P_k} = \begin{bmatrix} 1 & 1 \\ 0 & 1 \end{bmatrix}P_{k-1}\begin{bmatrix} 1 & 0 \\ 1 & 1 \end{bmatrix} Pk​ˉ​=[10​11​]Pk−1​[11​01​] x ^ k = x ^ k ˉ + K k ( z k − [ 1 0 ] x ^ k ˉ ) \hat{x}_k = \bar{\hat{x}_k} + K_k(z_k - \begin{bmatrix} 1 & 0 \end{bmatrix}\bar{\hat{x}_k}) x^k​=x^k​ˉ​+Kk​(zk​−[1​0​]x^k​ˉ​)
P k = ( I − K k [ 1 0 ] ) P k ˉ P_k = (I - K_k\begin{bmatrix} 1 & 0 \end{bmatrix})\bar{P_k} Pk​=(I−Kk​[1​0​])Pk​ˉ​

注意其中 x ∈ R 2 × 1 x \in R^{2 \times 1} x∈R2×1 ， P ∈ R 2 × 2 P \in R^{2 \times 2} P∈R2×2 ， K ∈ R 2 × 1 K \in R^{2 \times 1} K∈R2×1 。

仿真实验

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

TIME(ms)123456
HEIGHT(V)127.0115.3110.972.450.70.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');

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

无比机智的永哥
关注
专栏目录
博途PLC信号处理系列之卡尔曼滤波程序(Kalman Filter)
RXXW_Dor的博客
07-21 2485
三菱系列PLC的卡尔曼滤波程序，可以参看下面这篇博文PLC滤波算法之卡尔曼滤波(kalmanFilter)代码+测试_RXXW_Dor的博客-CSDN博客_plc滤波kalman滤波是采用递归的方法解决线性滤波问题，只需要当前的测量值input（西门子PLC里比如我们的模拟量通道AIW0值）和前一个采样周期的估计值（上一个时刻的滤波输出结果last_out）就能进行状态估计，需要的存储空间小，每一步的计算量小。下面我们通过程序代码一一给大家展示下。注意。......
卡尔曼滤波
cookiesMonster
12-10 1052
卡尔曼滤波，利用k时刻的预测值、k时刻的测量值和相应误差，得出k时刻估计值（即最优值），使得估计值均方误差最小。卡尔曼滤波算法是一套递推估计算法，即可以通过计算机迭代的过程求解（由上个状态求得当前状态）。 一、离散卡尔曼滤波算法 理解以下值：     测量值z(k) ——由测量数据直接得到     预测值 x(k|k-1) ——在一些文章中被称为先验      估计值 x(k|k
卡尔曼滤波的实例集合
08-10
卡尔曼滤波的实例集合，适用于各种预测、数值估计、拟合等问题的解决，有详细的代码辅助说明。
【算法】卡尔曼滤波
最新发布
wnm23的专栏
07-31 1286
卡尔曼滤波是一种强大的递归滤波器，适用于处理线性或非线性动态系统中的不确定性。卡尔曼滤波的原理、实现和应用场景，多语言代码示例。在实际应用中，卡尔曼滤波可以帮助我们准确地估计系统状态，提高决策和控制的精度。随着技术的发展，卡尔曼滤波及其变种在各个领域的应用将更加广泛。
【卡尔曼滤波】卡尔曼滤波简单实例
一些笔记
10-24 3879
卡尔曼滤波的一个简单示例，有助于对其的理解。
卡尔曼滤波的使用举例
VictorHan01的博客
07-06 8820
卡尔曼滤波（Kalman filtering）一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。 顺带提一下协方差矩阵，下面会用到。 协方差矩阵： 以下来源：https://blog.csdn.net/su_bao/article/details/80997077 ...
一个应用实例详解卡尔曼滤波及其算法
12-05
一个应用实例详解卡尔曼滤波及其算法实现,为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。
卡尔曼滤波DSP实现.zip_DSP 卡尔曼滤波_DSP 滤波_dsp卡尔曼滤波_卡尔曼_卡尔曼滤波 DSP
07-14
卡尔曼滤波是一种基于数学预测的优化估计方法，广泛应用于信号处理、控制理论和许多其他领域，尤其是在存在噪声和不确定性的情况下。在这个特定的压缩包文件中，重点是将卡尔曼滤波算法实现在数字信号处理器...
卡尔曼滤波与自适应卡尔曼滤波matlab例程.zip
07-04
该压缩包文件"卡尔曼滤波与自适应卡尔曼滤波matlab例程.zip"包含了基于MATLAB实现的卡尔曼滤波和自适应卡尔曼滤波的示例代码。MATLAB是一款强大的数学计算软件，适合进行复杂的数值运算和数据分析，是实现卡尔曼滤波...
卡尔曼滤波的小例子 很实用 初学者 MATLAB m文件编辑
09-19
卡尔曼滤波的小例子 很实用 初学者 MATLAB m文件编辑
卡尔曼滤波实例
07-19
学习卡尔曼滤波必备
一个应用实例详解卡尔曼滤波及其算法实现
12-11
直观讲述卡拉曼滤波，容易充分理解卡拉曼滤波
kalman滤波超详细讲解加小例子
01-06
kalman滤波超详细讲解加小例子，非常适合初学者和研究人员
卡尔曼滤波-matlab工具箱.rar_J552_matlab 目标跟踪_solveeb9_卡尔曼工具箱_卡尔曼滤波 matl
07-15
卡尔曼滤波是一种在噪声环境下估计系统状态的统计方法，由数学家鲁道夫·卡尔曼提出，广泛应用于工程、导航、控制理论、信号处理等多个领域。在MATLAB环境中，有专门的卡尔曼滤波工具箱，如描述中提到的"卡尔曼滤波-...
卡尔曼滤波示例程序
04-25
总结来说，"卡尔曼滤波示例程序"是理解和实践卡尔曼滤波算法的一个实用工具，它通过追踪二维点展示了滤波器如何处理噪声和不确定性，从而提供更精确的估计。配合OpenCV，用户可以直观地看到滤波效果，并且可以根据...
改进扩展卡尔曼滤波算法估算SOC模型,扩展卡尔曼滤波算法优缺点,matlab
09-10
标题中的“改进扩展卡尔曼滤波算法估算SOC模型”指的是使用一种优化的扩展卡尔曼滤波（EKF）方法来估计电池的荷电状态（State of Charge, SOC）。扩展卡尔曼滤波是一种在非线性系统中应用广泛的滤波算法，它通过线性...
卡尔曼滤波降噪代码示例
01-16
以下是一个使用卡尔曼滤波进行降噪的Python代码示例： ```python import numpy as np from scipy.linalg import inv def kalman_filter(data, Q, R): n = len(data) x = np.zeros((n,)) P = np.zeros((n,)) x...
评论 3
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值