KF_matlab.zip

clc clear %% %传感器信号读取 speed_profile1=xlsread('C:\Users\606\Desktop\坡度估算实验1\车速数据\test2-qucheng-speed data1.xlsx');%读取第一段道路车速文件 acceleration_profile1=xlsread('C:\Users\606\Desktop\坡度估算实验1\加速度数据\test2-qucheng-acceleration data1.xlsx');%读取第一段道路加速度文件 v1o=speed_profile1(:,1)./3.6;%第一段道路试验车速信号m/s t1=speed_profile1(:,2);%第一段道路时间信号 g=9.8;%重力加速度m/s2 delta_t=t1(2)-t1(1);%时间间隔s %% %传感器信号处理 as1=acceleration_profile1(:,2).*g;%第一段道路试验加速度信号（原）m/s2 tas1=acceleration_profile1(:,1);%第一段道路时间信号，加速度信号（原）对应 a1o=interp1(tas1,as1,t1,'PCHIP');%将第一段加速度信号进行插值处理，使加速度信号时间与车速信号时间对应 v1=smooth(v1o,10,'lowess'); a1=smooth(a1o,20,'lowess');%移动平均滤波，‘lowess’线性最小二乘滤波，'loess'加权线性最小二乘滤波'loess' %a1=medfilt1(a1o,20);%中值滤波 x1(1)=0; for i=1:length(v1)-1 x1(i+1)=x1(i)+v1(i)*delta_t; end figure(1) plot(t1,v1o,'b',t1,v1,'r') title('第一段坡道车速信号滤波对比') legend('车速原信号','车速滤波后信号'); xlabel('时间（s）');ylabel('车速（m/s）'); figure(2) plot(t1,a1o,'b',t1,a1,'r','LineWidth',1) title('第一段坡道加速度信号滤波对比') legend('加速度原信号','加速度滤波后信号'); xlabel('时间（s）');ylabel('加速度（m/s^2）'); %% %第一段坡道卡尔曼滤波估算 N1=length(t1);%计算连续N个时刻 n1=3;%状态维度，3个估计值 Xo1=zeros(n1,N1);%最优估计值迭代记录矩阵 Xs1=[0;0;0];%初始值 F1=[1 delta_t -g*delta_t;0 1 0;0 0 1];%状态转移/预测矩阵 H1=[1 0 0;0 1 0];%观测矩阵 P1=eye(n1); Q1=[1 0 0;0 1 0;0 0 1]; R1=[1e-2 0;0 1]; for i=2:N1 z1=[v1(i);a1(i)];%观测值 Xa1=F1*Xs1;%预测 P_1=F1*P1*F1'+Q1;% K1=P_1*H1'/((H1*P_1*H1'+R1));%卡尔曼增益 Xb1=Xa1+K1*(z1-H1*Xa1);%当前时刻最优估计值 Xo1(:,i)=Xb1;%记录 P1=(eye(3)-K1*H1)*P_1; %P=P_-K*H*P_;%更新 Xs1=Xb1;%更新 end v1_est=Xo1(1,:);%速度估计值 a1_est=Xo1(2,:);%加速度传感器读数估计值 angle1_est=asin(Xo1(3,:))/pi*180;%坡度估计值 figure(3) plot(t1,v1_est,'r',t1,v1,'b') title('第一段坡道车速信号卡尔曼估算对比') legend('车速估算值','车速真值'); xlabel('时间（s）');ylabel('车速（m/s）'); figure(4) plot(t1,a1_est,'r',t1,a1,'b') title('第一段坡道加速度信号卡尔曼估算对比') legend('加速度估算值','加速度真值'); xlabel('时间（s）');ylabel('加速度（m/s^2）'); figure(5) plot(x1,angle1_est,'r') title('第一段坡道坡度估算值') xlabel('路程（m）');ylabel('坡度角°');