% Copyright 2010 The MathWorks, Inc.function y = kalmanfilter(z) % z是可测变量的当前测量值%% (0)初始化dt=1;% 状态转移矩阵A=[ 1 0 dt 0 0 0;... % [x ] 0 1 0 dt 0 0;... % [y ] 0 0 1 0 dt 0;... % [Vx] 0 0 0 1 0 dt;... % [Vy] 0 0 0 0 1 0 ;... % [Ax] 0 0 0 0 0 1 ]; % [Ay]H = [ 1 0 0 0 0 0; 0 1 0 0 0 0 ]; % 测量矩阵% 噪声协方差Q = eye(6);R = 1000 * eye(2);% x_est是系统状态(后验状态)% p_est后验估计误差的协方差persistent x_est p_est if isempty(x_est) x_est = zeros(6, 1); % x_est=[x,y,Vx,Vy,Ax,Ay]' p_est = zeros(6, 6);end%% (1)预测x_prd = A * x_est; % 先验状态p_prd = A * p_est * A' + Q; % 先验估计误差的协方差%% (1.5)估计Kalman增益矩阵S = H * p_prd' * H' + R;B = H * p_prd';klm_gain = (S \ B)';%% (2)更新% 利用当前测量值、Kalman增益、后验估计、观测矩阵更新系统状态% 更新过程是一个以Kalman增益为权重的加权修正过程x_est = x_prd + klm_gain * (z - H * x_prd);p_est = p_prd - klm_gain * H * p_prd;%% (3)通过KF的测量输出y = H * x_est;end % of the function