clear all
clc
a=1;
dt=0.1;
duration=20;
measnoise = 10;
accelnoise = 1.5;
%http://blog.csdn.net/boksic
F = [1 dt; 0 1]; % transition matrix
H = [1 0]; % 测量矩阵
xk = [0; 0]; %初始状态，位置速度都为0
xhat = xk; % 初始估计值
Q = accelnoise^2 * [dt^4/4 dt^3/2; dt^3/2 dt^2]; % process noise covariance
P = Q; % initial estimation covariance
R = measnoise^2; % measurement error covariance
% set up the size of the innovations vector
Inn = zeros(size(R));
pos = []; % true position array
poshat = []; % estimated position array
posmeas = []; % measured position array
Counter = 0;
for t = 0 : dt: duration
Counter = Counter + 1;
%产生过程噪声
ProcessNoise = accelnoise * randn * [(dt^2/2); dt];
%模拟产生真实状态X
xk = F * xk + ProcessNoise;
%测量噪声
MeasNoise = measnoise * randn;
%模拟测量结果
zk = H * xk + MeasNoise;%工程实际中的已知测量值
% 以下为滤波
% Innovation，测量值与估计值的差
Inn = zk - H * xhat;
% Covariance of Innovation
s = H * P * H' + R;
% Gain matrix，可看做根据测量值与估计值的可信度得到的权重系数
K = a * P * H' * inv(s);
% 计算最优估计值
%F*xhat为根据牛顿定律推算当前值
%K*Inn为修正项
xhat = F * xhat + K * Inn;
% 更新标准差矩阵，从而不断的循环
P = F * P * F' + Q - F * P * H' * inv(s) * H * P * F';
pos = [pos; xk(1)];
posmeas = [posmeas; zk];
poshat = [poshat; xhat(1)];
end
%显示结果
t = 0 : dt : duration;
t = t';
plot(t,pos,'r',t,poshat,'g',t,posmeas,'b');
grid;
xlabel('时间 (sec)');
ylabel('位置 (feet)');
title('Kalman Filter Performance');
legend('真实值','预测值','测量值')
