matlab一维线性Kalman滤波.doc

(word完整版)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）;％本系统状态为一维滤波
％模拟房间温度的测量值
for k=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滤波
for k=2:N
X_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估计值与真实值之间的偏差
for k=1：N
Err_Messure（k)=abs(Z（k)—X（k）);
Err_Kalman（k）=abs（Xkf（k）-X(k））；
end
t=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=CON＊ones(1,N）;
Xkf=zeros(1，N）；
P=zeros（1，N);
X（1)=909;
P(1）=156；%（923-909）^2
Z(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=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;
end

Err_Messure=zeros(1，N)；
Err_Kalman=zeros（1,N）;
for k=1:N
Err_Messure（k)=abs（Z(k）—Xkf（k））;
end
t=1：N;
figure
plot（t,Z,'—ko’,t，Xkf，’-g*');
legend（'观测值’,'Kalman滤波值’）;
xlable('采样时间’)；
ylable('测量值/mm’);
figure
plot（t,Err_Messure,'-b')；
legend('测量偏差'，'Kalman滤波偏差’)；
xlable('采样时间’);
ylable('测量偏差值/mm’);