kaerman.rar_数据滤波_跟踪滤波

%卡尔曼滤波 跟踪 滤波 %1.产生目标测量序列z(n)，并利用三个测量值初始化滤波器 T = 1; %采样周期 var_x = 100; %观测噪声误差 z = x_true(1,:) + sqrt(var_x) * randn(1,length(x - true(1,:))); %x_true存放目标真实状态 X3 = [z(3), (z(3) - z(2))/T, ((z(3) - z(2))/T - (z(2) - z(1))/T)/T]'; P3 = [var_x, var_x/T, var_x/T^2; var_x/T, var_x/T^2, 3*var_x/T^3; var_x/T^2, 3*var_x/T^3, 4*var_x/T^4;]; %2.对滤波器参数进行赋值 F = [1, T, T^2/2; 0, 1, T; 0, 0, 1]; %状态转移矩阵 Tao = [T^2/2, T, 1]'; %过程噪声输入矩阵 Q1 = 9; %过程噪声输入矩阵 Q2 = var_x; %观测噪声误差 C = [1, 0, 0]; %3.卡尔曼滤波 N = 100; %采样次数 X_esti = X3; %状态估计值 X_esti_all = X3; %存放个时刻的状态估计值 P_esti = P3; %估计误差自相关矩阵 P_esti_all = P3; %存放各时刻误差自相关矩阵 for k = 4:N X_pre = F * X_esti; %状态预测 Z_pre = C * X_pre; %观测预测 P_pre = F * P_esti * F' + Tao * Q1 * Tao'; %预测误差自相关矩阵 A = C * P_pre * C' +Q2; %新息自相关矩阵 K = P_pre * C' * inv(A); %增益矩阵 X_esti = X_pre + K * (Z(k)-Z_pre); %状态估计 P_esti = (eye(3) -K * C) * P_pre; %估计误差自相关矩阵 X_esti_all = [X_esti_all, X_esti]; P_esti_all = [P_esti_all, P_esti]; end %重复上述过程， 进行100次试验，即可获得平均结果！