卡尔曼滤波以及MATLAB使用

卡尔曼滤波是一个很常用的滤波算法,与维纳滤波相比有很多长处。这里我们把Kalman Filter简称为KF。KF的基本思想是:采用信号、噪声、状态空间模型,利用前一时刻的状态最优估计值及其误差方差估计和现时刻的量测值来更新对状态变量的估计,求出现在时刻的最优估计值。

说白点就是对现在时刻的估计(可能是同时估计好几个变量)是取决于前一时刻估计误差和现在时刻的某个观测值。通过不断的预测和实测来修正自己的估计值,最后达到一个理想的平稳状态。

卡尔曼滤波的具体原理和推导过程,有点复杂。某些细节我也没弄明白,不过这并不影响我们写程序和使用它。如果你看了我写的这个小分享,表明你对卡尔曼滤波有一定的了解了,否则你也不会看这种玩意。如果真的不了解,那可以查找下相关资料。

卡尔曼滤波的5大公式:

这五个公式公式可要注意他们的下标了,下面我们从上至下依次说下这五个公式的意思

第一个公式就是说我们研究的变量在这一时刻与上一时刻的具体关系。那个Fai矩阵就是系数对应关系。可以看到这里就把卡尔曼滤波的使用范围限定在线性系统中了,基本的卡尔曼滤波器是不能用于非线性系统的。

第二个公式叫方差先验估计,说的这么文艺,其实就是计算估计值和实际值的方差大小,以评估现在的估计的误差是不是合适。

第三个公式叫增益矩阵,前面已经说过,卡尔曼滤波根据当前观测变量的变化趋势来估计一时刻变量的值,这个增益矩阵就是指我们定的这个变化的“度”的大小。

第四个就是求要估计的值了。

第五个是修正估计值与实际值的方差。

这5个公式是卡尔曼滤波的关键,下面就结合一个实例在说下卡尔曼滤波的程序如何写~交大的同学,如果你们选了最优估计这门课,然后看到这个,嘿嘿,便宜你们了。

应用:卡尔曼滤波在目标测量中的应用

某火箭在空中沿直线做匀加速飞行,加速度为  ,发动机推力的波动一定会引起加速度波动,雷达以间隔时间不断测量该火箭距离远点的距离  ,由于雷达的测量噪声使得火箭距离测量值   中含有噪声。现在欲通过雷达对火箭距离的测量数据求得火箭的真实距离、速度和加速度。

  1. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  2. %     设置初始化信息
  3. %  N:设置卡尔曼滤波器追踪点数
  4. %  r:设置估计变量个数,这里r=3
  5. %  s:被追踪的火箭的距离,初始值为1000m
  6. %  v:火箭的速度,初始值为50m/s
  7. %  a:火箭的加速度,初始值为20m/s2,此时加速度默认为不变
  8. %  T: 雷达的扫描间隔,此时设为1秒
  9. %  wt: 系统噪声,方差为20
  10. %  vt: 量测噪声,方差为16
  11. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  12. clear all;
  13. close all;
  14. N = 100;
  15. r = 3;
  16. t = 1:1:N;
  17. T = 1;
  18. s = zeros(1,N);
  19. v = zeros(1,N);
  20. a = zeros(1,N);
  21. a(t) = 20;
  22. s0 = 1000;
  23. v0 = 50;
  24. for n = 1:N
  25.     v(n) = v0 + a(n)*n;
  26.     s(n) = 1000+v0*n+0.5*a(n)*n*n;
  27. end
  28. wt = randn(1,N);
  29. wt = sqrt(4)*wt./std(wt);
  30. s = s + wt;
  31. v = v + wt;
  32. a(t) = a(t) + wt(t);
  33. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  34. % 卡尔曼滤波部分,继承之前初始化变量
  35. % A:转移矩阵
  36. % H:量测矩阵
  37. % Qk:系统噪声矩阵
  38. % Rk:量测噪声矩阵
  39. % P0:均方误差矩阵初始值
  40. % Y:火箭的状态矩阵,由k_s,k_v,k_a组成
  41. % Y0:状态矩阵的初始值
  42. %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  43. Y0 = [900 0 0]’;
  44. Y = [Y0 zeros(r,N-1)];
  45. A = [1 1 0;0 1 1;0 0 1];
  46. H = [1 0 0];
  47. Qk = [0 0 0;0 0 0;0 0 20];
  48. Rk = 16;
  49. P0 = [30 0 0;0 20 0;0 0 10];
  50. P1 = P0;
  51. P2 = zeros(r,r);
  52. for k = 1:N
  53.     Y(:,k) = A*Y(:,k);
  54.     P2 = A*P1*A’+Qk;
  55.     Kk = P2*H’*inv(H*P2*H’+Rk);
  56.     Y(:,k+1) = Y(:,k)+Kk*(s(:,k)-H*Y(:,k));
  57.     P1 = (eye(r,r)-Kk*H)*P2;
  58. end
  59. k_s = Y(1,:);
  60. k_v = Y(2,:);
  61. k_a = Y(3,:);
  62. subplot(3,1,1);
  63. plot(t,s(t),’-‘,t,k_s(t),’o’);
  64. title(‘距离’);
  65. legend(‘实际值’,’估计值’);
  66. xlabel(‘t’);
  67. ylabel(‘s’);
  68. subplot(3,1,2);
  69. plot(t,v(t),t,k_v(t),’+’);
  70. title(‘速度’);
  71. legend(‘实际值’,’估计值’);
  72. xlabel(‘t’);
  73. ylabel(‘v’);
  74. subplot(3,1,3);
  75. plot(t,a(t),t,k_a(t),’-x’);
  76. title(‘加速度’);
  77. legend(‘实际值’,’估计值’);
  78. xlabel(‘t’);
  79. ylabel(‘a’);
  80. axis([0,N,0,30]);

具体细节不讲了。运行后,就得到我们想要的结果了~

 

一个小总结,希望对你有帮助。

发布者

harifun

小学的时候,说自己要当一名科学家!那时候,看到新闻联播在宣传“四化”,立志要为现代化作出贡献!高中立志要做机器人,本科和硕士学的自动化!毕业进入华为,因为是测试岗位而离开,然后进入创业公司做工业扫地机器人和服务式机器人!A轮完成数千万级融资后因家庭原因离开上海,回武汉进入一家上市公司从事激光雷达设计工作!机器人是我一直以来的追求,希望有一天我能实现我的理想!

发表评论

电子邮件地址不会被公开。 必填项已用*标注