卡尔曼滤波与最小二乘法
基于MATLAB 的卡尔曼滤波与最小二乘滤波仿真实验设计
一、实验原理：卡尔曼滤波器是一个最优化自回归数据处理算法，对于解决很大部分的问题，他是最优、效率最高甚至是最有用的，其核心内容就是他的5条公式，具体见实验内容中详细介绍。最小二乘法的核心思想是对于一系列观察值，找出一条最优化曲线使其与每个观察值之间差值的平方和最小。
二、实验目的：运用MATLAB 进行仿真实验可以更清楚直观系统的理解滤波理论设计思想及其方法，并提高学生的科研能力和水平。通过MATLAB 编程输入算法，利用其强大的矩阵运算能力和优秀的图形界面功能输出结果，对增强教学效果可以起到很有效的作用，有利于学生对算法进行更深入的理解，同时在设计仿真实验的过程中还可以提高学生使用MATLAB 的技能。
三、实验要求：
设计原始信号，随机产生噪声信号，设计代码，分别使用卡尔曼滤波和最小二乘滤波编程，更换相应参数生成不同的滤波效果，进行科学地分析。
四 、实验内容及程序：
实验内容：
假设研究对象是一个房间的温度。根据经验判断，这个房间的温度是恒定的，但经验判断有一定的上下偏差几度，把偏差看成是高斯白噪声，也就是这些偏差跟前后时间是没有关系的而且符合高斯分配。另外，房间中用温度计测量，但温度计有一定的精确度，与实际值有偏差。目前该房间有两个温度值：根据经验的预测值（系统预测值）和温度计值（测量值）。 假设该房间是23摄氏度，同时该值的高斯噪声的偏差是5度（如果上一时刻估算最有温度值的偏差是3，预测值是4，则按照一类不确定度计算，平方和再开方为5）如果温度计该时刻测量值是25度，同时该值的偏差是4度。
卡尔曼滤波：
引入一个离散控制过程的系统。该系统可用一个线性随机微分方程来描述：
X(k)=A X(k-1)+B U(k)+W(k)，再加上系统的测量值：Z(k)=H X(k)+V(k) ，X(k)是k 时刻的系统状态，U(k)是k 时刻对系统的控制量，如果没有控制量，它可以为0。A 和B 是系统参数，对于多模型系统，他们为矩阵。Z(k)是k 时刻的测量值，H 是测量系统的参数，对于多测量系统，H 为矩阵。W(k)和V(k)分别表示过程和测量的噪声。他们被假设成高斯白噪声
(White Gaussian Noise)，他们的covariance 分别是Q ，R （这里我们假设他们不随系统状态变化而变化）。其核心的五个公式为：
x (k |k -1) =A x (k -1|k -1)
p (k |k -1) =A p (k -1|k -1) A ' +Q
x (k |k ) =x (k |k -1) +K (k )[z (k ) -H x (k |k -1)] ^^^^^^^
K (k ) =p (k |k -1) ^^-1H p (k |k -1) H ' +R
^^ p (k |k ) =(1-K (k ) H ) p (k |k )
程序代码：
clear
%赋初值, 系统参数设置
A=1; %这个房间的温度是跟前一时刻的温度相同则A=1
H=1;%测量系统的参数
Q=4;%量测噪声
R=1;%系统噪声
s(1)=25; %假设房间当前真实温度为25度
N=100;%设置100个数
w=randn(1,N);%产生正态分布伪随机信号，即模拟随机产生噪声信号
v=randn(1,N);
v(1)=0;
for k=2:N;
s(k)=A*s(k-1)+w(k-1);%没有控制量u （k ）=0
z(k)=H*s(k)+v(k-1);
end
x(1)=25;
for t=2:N;
xpred(t)=A*x(t-1);
vpred(t)=A*v(t-1)*A'+Q;
k(t)=(vpred(t)*H)/(H*vpred(t)*H'+R);
x(t)=xpred(t)+k(t)*(z(t)-H*xpred(t));
v(t)=(1-k(t)*H)*vpred(t);
end
p=polyfit(x,z,3);
f=polyval(p,x);
t=1:N;
plot(t,x,'r',t,z,'g',t,s,'k',t,f,'b*-');
xlabel('时间/s')
ylabel('温度/℃')
grid on
legend('卡尔曼滤波',' 观测值',' 真实值',' 最小二乘滤波')
title('卡尔曼滤波与最小二乘滤波仿真图')
1 重新设置噪声参数：
clear
%赋初值, 系统参数设置
A=1; %这个房间的温度是跟前一时刻的温度相同则A=1
H=1;%测量系统的参数
Q=2;%重置测量噪声
R=1;%系统噪声
s(1)=25; %假设房间当前真实温度为25度
N=100;%设置100个数
w=randn(1,N);%产生正态分布伪随机信号，即模拟随机产生噪声信号
v=randn(1,N);
v(1)=0;
for k=2:N;
s(k)=A*s(k-1)+w(k-1);%没有控制量u （k ）=0
z(k)=H*s(k)+v(k-1);
end
x(1)=25;
for t=2:N;
xpred(t)=A*x(t-1);
vpred(t)=A*v(t-1)*A'+Q;
k(t)=(vpred(t)*H)/(H*vpred(t)*H'+R);
x(t)=xpred(t)+k(t)*(z(t)-H*xpred(t));
v(t)=(1-k(t)*H)*vpred(t);
end
p=polyfit(x,z,3);
f=polyval(p,x);
t=1:N;
plot(t,x,'r',t,z,'g',t,s,'k',t,f,'b*-');
xlabel('时间/s')
ylabel('温度/℃')
grid on
legend('卡尔曼滤波',' 观测值',' 真实值',' 最小二乘滤波')
title('重置量测噪声卡尔曼滤波与最小二乘滤波仿真图')
2 重新设置拟合阶数：
clear
%赋初值, 系统参数设置
A=1; %这个房间的温度是跟前一时刻的温度相同则A=1
H=1;%测量系统的参数
Q=4;%重置量测噪声
R=1;%系统噪声
s(1)=25; %假设房间当前真实温度为25度
N=100;%设置100个数
w=randn(1,N);%产生正态分布伪随机信号，即模拟随机产生噪声信号
v=randn(1,N);
v(1)=0;
for k=2:N;
s(k)=A*s(k-1)+w(k-1);%没有控制量u （k ）=0
z(k)=H*s(k)+v(k-1);
end
x(1)=25;
for t=2:N;
xpred(t)=A*x(t-1);
vpred(t)=A*v(t-1)*A'+Q;
k(t)=(vpred(t)*H)/(H*vpred(t)*H'+R);
x(t)=xpred(t)+k(t)*(z(t)-H*xpred(t));
v(t)=(1-k(t)*H)*vpred(t);
p=polyfit(x,z,4);
f=polyval(p,x);
t=1:N;
plot(t,x,'r',t,z,'g',t,s,'k',t,f,'b*-');
xlabel('时间/s')
ylabel('温度/℃')
grid on
legend('卡尔曼滤波',' 观测值',' 真实值',' 最小二乘滤波')
title('重置拟合阶数卡尔曼滤波与最小二乘滤波仿真图')
3 重置测量噪声和拟合阶数：
clear
%赋初值, 系统参数设置
A=1; %这个房间的温度是跟前一时刻的温度相同则A=1
H=1;%测量系统的参数
Q=2;%重置量测噪声
R=1;%系统噪声
s(1)=25; %假设房间当前真实温度为25度
N=100;%设置100个数
w=randn(1,N);%产生正态分布伪随机信号，即模拟随机产生噪声信号
v=randn(1,N);
v(1)=0;
for k=2:N;
s(k)=A*s(k-1)+w(k-1);%没有控制量u （k ）=0
z(k)=H*s(k)+v(k-1);
end
x(1)=25;
for t=2:N;
xpred(t)=A*x(t-1);
vpred(t)=A*v(t-1)*A'+Q;
k(t)=(vpred(t)*H)/(H*vpred(t)*H'+R);
x(t)=xpred(t)+k(t)*(z(t)-H*xpred(t));
v(t)=(1-k(t)*H)*vpred(t);
end
p=polyfit(x,z,4);
f=polyval(p,x);
t=1:N;
plot(t,x,'r',t,z,'g',t,s,'k',t,f,'b*-');
xlabel('时间/s')
ylabel('温度/℃')
grid on
legend('卡尔曼滤波',' 观测值',' 真实值',' 最小二乘滤波')
title('重置量测噪声以及拟合阶数卡尔曼滤波与最小二乘滤波仿真图')
4 重置系统噪声：
%赋初值, 系统参数设置
A=1; %这个房间的温度是跟前一时刻的温度相同则A=1
H=1;%测量系统的参数
Q=4;%量测噪声
R=5;%系统噪声
s(1)=25; %假设房间当前真实温度为25度
N=100;%设置100个数
w=randn(1,N);%产生正态分布伪随机信号，即模拟随机产生噪声信号
v=randn(1,N);
v(1)=0;
for k=2:N;
s(k)=A*s(k-1)+w(k-1);%没有控制量u （k ）=0
z(k)=H*s(k)+v(k-1);
end
x(1)=25;
for t=2:N;
xpred(t)=A*x(t-1);
vpred(t)=A*v(t-1)*A'+Q;
k(t)=(vpred(t)*H)/(H*vpred(t)*H'+R);
x(t)=xpred(t)+k(t)*(z(t)-H*xpred(t));
v(t)=(1-k(t)*H)*vpred(t);
end
p=polyfit(x,z,4);
f=polyval(p,x);
t=1:N;
plot(t,x,'r',t,z,'g',t,s,'k',t,f,'b*-');
xlabel('时间/s')
ylabel('温度/℃')
grid on
legend('卡尔曼滤波',' 观测值',' 真实值',' 最小二乘滤波')
title('重置系统噪声卡尔曼滤波与最小二乘滤波仿真图')
5 重置测量系统的参数：
clear
%赋初值, 系统参数设置
A=1; %这个房间的温度是跟前一时刻的温度相同则A=1
H=3;%测量系统的参数
Q=4;%量测噪声
R=1;%系统噪声
s(1)=25; %假设房间当前真实温度为25度
N=100;%设置100个数
w=randn(1,N);%产生正态分布伪随机信号，即模拟随机产生噪声信号
v=randn(1,N);
v(1)=0;
for k=2:N;
s(k)=A*s(k-1)+w(k-1);%没有控制量u （k ）=0
z(k)=H*s(k)+v(k-1);
end
x(1)=25;
for t=2:N;
xpred(t)=A*x(t-1);
vpred(t)=A*v(t-1)*A'+Q;
k(t)=(vpred(t)*H)/(H*vpred(t)*H'+R);
x(t)=xpred(t)+k(t)*(z(t)-H*xpred(t));
v(t)=(1-k(t)*H)*vpred(t);
end
p=polyfit(x,z,4);
f=polyval(p,x);
t=1:N;
plot(t,x,'r',t,z,'g',t,s,'k',t,f,'b*-');
xlabel('时间/s')
ylabel('温度/℃')
grid on
legend('卡尔曼滤波',' 观测值',' 真实值',' 最小二乘滤波')
title('重置测量系统参数卡尔曼滤波与最小二乘滤波仿真图')
注：MATLAB 运行结果见附有的图
五、实验结果分析比较：
在卡尔曼滤波的应用中若将时间因素固定，则卡尔曼滤波退化为经典最小二乘法. 在通常情况下必须进行迭代计算，计算过程比经典最小二乘法复杂，但解的精度不会有任何改善。使用最小二乘的优点是：不需要掌握太多的系统知识，仅仅知道观测数据就可以进行滤波，应用广泛，于对系统不够了解的情况下使用最小二乘比较有优势。卡尔曼滤波必须对系统要充分了解，通过学习的方法、系统分析法、采样法等获取系统的相关参数，然后进行滤波，对系统了解的越多，则滤波精度越高。这就使得卡尔曼滤波必须在对系统有一定的了解下才能使用，否则精度太低。改变量测噪声参数会影响卡尔曼滤波效果，量测噪声在一定范围内会影响卡尔曼滤波效果；改变系统噪声也会影响卡尔曼滤波效果，增大系统参数会使滤波效果变差；改变测量系统的参数，会使观测值过大，但真实值和卡尔曼滤波效果能较好的实现吻合；改变系统参数不影响最小二乘滤波结果，但是改变拟合阶数会影响最小二乘的拟合效果，阶数越高拟合的精度越高。