matlab一维线性Kalman滤波.doc

(word完整版)matlab一维线性Kalman滤波噪声矩阵的处理1、假设温度计的测量误差为0。5度,从出厂说明书可得知此温度计的方差为0。25。所以,温度计的每次测量值都是有误差的，即R=0。25。2、假定第k1时刻的温度值测得为23。9度，房间真实温度为24.0度，测量值的偏差为0.1度，即协方差P(k1)=0。12。3、假设测量温度时，外界的天气是多云，阳光照射时有时无，同时房间不是密封的,可能有微小的空气流动，即引入过程噪声W（k)，其方差为Q，大小假定为Q=0。01(假如不考虑过程噪声的影响,即温度是恒定，则Q=0，但这不可能）。4、F、G、H为系统状态转移矩阵，与系统维数有关.此为一维温度数据估计，故都为1。%功能说明：Kalman滤波用于一维温度数据测量系统中N=120;%采样点个数CON=25;室内温度理论值,在这个理论值的基础上受过程噪声会有波动对状态和测量初始化Xexpect=CONones（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）;本系统状态为一维滤波模拟房间温度的测量值for k=2:N 第一步：随时间推移，房间真实温度发生变化 k时刻房间的真实温度，对于温度计来说，这个真实值是不知道的 X（k)=F*X（k-1）+GW（k-1); 第二步：随着时间推移，获取实时数据 温度计对k时刻房间温度的测量，Kalman滤波是站在温度计角度进行的 它不知道此刻真实状态X（k)，只能利用本次测量值Z（k）和上一次估计值 Xkf(k）来做处理,其目标是最大限度地降低测量噪声R的影响,尽可能地逼近X(k) Z（k）=H*X(k)+V（k);end 第三步:Kalman滤波for k=2:N X_pre=F*Xkf（k1）；状态预测 P_pre=FP(k1）*F+Q;协方差预测 Kg=P_pre*inv（H*P_preH+R）；%计算Kalman增益 e=Z（k）-H*X_pre;新息 Xkf(k)=X_pre+Kge;状态更新 P(k)=（IKgH)*P_pre；协方差更新end%计算误差Err_Messure=zeros（1,N)；测量值与真实值之间的偏差Err_Kalman=zeros(1，N);%kalman估计值与真实值之间的偏差for k=1：N Err_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=909 938 943 929 914 904 929 918;N=length（Z）；CON=923;Xexpect=CONones(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)；for k=2：N X_pre=FXkf(k-1)； P_pre=FP（k1)*F+Q； Kg=P_preinv(H*P_pre*H+R); e=Z（k）-H*X_pre; Xkf(k）=X_pre+Kg*e; P（k）=（IKgH）P_pre;endErr_Messure=zeros(1，N)；Err_Kalman=zeros（1,N）;for k=1:N Err_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);