标准Kalman.rar_kalman_matlab_operationa55_三个标准Kalman滤波程序

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('真实值','预测值','测量值')