matlab一维线性Kalman滤波

噪声矩阵的处理1、假设温度计的测量误差为0.5度，从出厂说明书可得知此温度计的方差为0.25。所以，温度计的每次测量值都是有误差的，即R=0.25。2、假定第k-1时刻的温度值测得为23.9度，房间真实温度为24.0度，测量值的偏差为0.1度，即协方差P(k-1)=0.1^2。3、假设测量温度时，外界的天气是多云，阳光照射时有时无，同时房间不是密封的，可能有微小的空气流动，即引入过程噪声W(k)，其方差为Q，大小假定为Q=0.01（假如不考虑过程噪声的影响，即温度是恒定，则Q=0，但这不可能）。4、F、G、H为系统状态转移矩阵，与系统维数有关。此为一维温度数据估计，故都为1。%功能说明：Kalman滤波用于一维温度数据测量系统中N=120;%采样点个数CON=25;%室内温度理论值，在这个理论值的基础上受过程噪声会有波动%对状态和测量初始化Xexpect=CON*ones(1,N);%期望的温度是恒定的25度，但真实温度不可能会不变动X=zeros(1,N);%房间各个采样时刻点的真实温度值Xkf=zeros(1,N);%Kalman滤波处理的状态，即估计值Z=zeros(1,N);%温度计实际测量值P=zeros(1,N);%赋初值X(1)=25.1;%假如房间的初始温度为25.1度P(1)=0.01;%初始值的协方差Z(1)=24.9;Xkf(1)=Z(1);%将初始测量值24.9度作为滤波器的初始估计值%噪声，下面默认环境噪声为高斯白噪声，参数设置为Q，R，也可以根据实际情况设置大小Q=0.01;R=0.25;W=sqrt(Q)*randn(1,N);%方差决定噪声大小V=sqrt(R)*randn(1,N);%方差决定噪声大小%系统矩阵F=1;G=1;H=1;I=eye(1);%本系统状态为一维滤波%模拟房间温度的测量值fork=2:N%第一步：随时间推移，房间真实温度发生变化%k时刻房间的真实温度，对于温度计来说，这个真实值是不知道的X(k)=F*X(k-1)+G*W(k-1);%第二步：随着时间推移，获取实时数据%温度计对k时刻房间温度的测量，Kalman滤波是站在温度计角度进行的%它不知道此刻真实状态X(k)，只能利用本次测量值Z(k)和上一次估计值%Xkf(k)来做处理，其目标是最大限度地降低测量噪声R的影响，尽可能地逼近X(k)Z(k)=H*X(k)+V(k);end%第三步：Kalman滤波fork=2:NX_pre=F*Xkf(k-1);%状态预测P_pre=F*P(k-1)*F'+Q;%协方差预测Kg=P_pre*inv(H*P_pre*H'+R);%计算Kalman增益e=Z(k)-H*X_pre;%新息Xkf(k)=X_pre+Kg*e;%状态更新P(k)=(I-Kg*H)*P_pre;%协方差更新end%计算误差Err_Messure=zeros(1,N);%测量值与真实值之间的偏差Err_Kalman=zeros(1,N);%kalman估计值与真实值之间的偏差fork=1:NErr_Messure(k)=abs(Z(k)-X(k));Err_Kalman(k)=abs(Xkf(k)-X(k));endt=1:N;figure(1);plot(t,Xexpect,'-b',t,X,'-r',t,Z,'-ko',t,Xkf,'-g*');legend('期望值','真实值','观测值','Kalman滤波值');xlable('采样时间');ylable('测量值');%figure(2);%plot(t,Err_Messure,'-b',t,Err_Kalman,'-k*');%legend('测量偏差','Kalman滤波偏差');%xlable('采样时间');%ylable('测量偏差值/mm');结果%功能说明：Kalman滤波用于一维数据测量系统中Z=[909938943929914904929918];N=length(Z);CON=923;Xexpect=CON*ones(1,N);Xkf=zeros(1,N);P=zeros(1,N);X(1)=909;P(1)=156;%（923-909）^2Z(1)=909;Xkf(1)=Z(1);Q=0.01;R=0.25;W=sqrt(Q)*randn(1,N);V=sqrt(R)*randn(1,N);F=1;G=1;H=1;I=eye(1);fork=2:NX_pre=F*Xkf(k-1);P_pre=F*P(k-1)*F'+Q;Kg=P_pre*inv(H*P_pre*H'+R);e=Z(k)-H*X_pre;Xkf(k)=X_pre+Kg*e;P(k)=(I-Kg*H)*P_pre;endErr_Messure=zeros(1,N);Err_Kalman=zeros(1,N);fork=1:NErr_Messure(k)=abs(Z(k)-Xkf(k));endt=1:N;figureplot(t,Z,'-ko',t,Xkf,'-g*');legend('观测值','Kalman滤波值');xlable('采样时间');ylable('测量值/mm');figureplot(t,Err_Messure,'-b');legend('测量偏差','Kalman滤波偏差');xlable('采样时间');ylable('测量偏差值/mm');