使用卡尔曼滤波预测一个人的运动状态

clc clear close all %% 对有关的参数进行设计 N = 1000 ; %设计数据长度为N t = (1:N); %生成时间轴 E = eye (2); %对进行kalman滤波需要知道的前提条件进行设置 %% 计算pk-（先验误差的协方差）时需要的参数 A = [1,1;0,1]; %系统矩阵，状态转移矩阵 Q = [0,0;0,0]; %预测噪声方差为0，忽略人在运行时的阻力 W = sqrt(Q)*randn(2,N); %预测模型中的噪音 %% 计算kk（卡尔曼增益）时需要的参数 H = [1,0;0,1]; %系统矩阵，状态转移矩阵 R = 1; %测量模型的方差 V = sqrt(R)*randn(1,N); %观测模型中的噪音 %% 给定初始值 X = zeros(2,N); %初始化，物体的真实状态 X(:,1) = [95;1] ;%初始化位移和速度，分别为95、1 P0 = [10,0;0,1]; %初始化误差 Z(:,1) = H*X(:,1); %初始化观测值 Xkf = zeros(2,N); x_filter = zeros(2,N); %% 进行卡尔曼滤波的计算 for k = 2:N X(:,k) = A*X(:,k-1) + W(k); %物体的位移和速度的状态方程 Z(:,k) = H*X(:,k) + V(k); %传感器对目标进行观测 X_pre = A * Xkf(:,k-1); %状态预测 P_pre = A*P0*A'+Q; %协方差预测pk- Kk = P_pre *H'*pinv(H*P_pre*H'+R); %计算卡尔曼增益 Xkf(:,k) = X_pre +Kk*(Z(k)-H*X_pre); %更新状态 P0 = (E-Kk*H)*P_pre; %方差更新 % k(k) = Kk; x_filter(:,k) = Xkf(:,k); err_P(k,1) = P0(1,1); err_P(k,2) = P0(2,2); end %% 展示 %plot(4,Kk); %legend('Kk'); figure(2) plot(t,X,'r',t,Z,'g',t,x_filter,'b') legend('real','measure','filter')