babybabytellmewhy / blog_20131130_1_348785

来自CSDN博客:卡尔曼滤波及其MATLAB程序 http://blog.csdn.net/qq514218063/article/details/17034813#

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