卡尔曼滤波的一阶仿真（温度）直接上matlab代码

卡尔曼滤波有五个公式，就不展示了，直接上mabtlab程序

%卡尔曼滤波在一维温度测量中的应用
N=120;%采样点的个数
T=25;%房间温度的理论值，假设不受干扰的话。
X_expect=T*ones(1,N);%每个采样点如果没有干扰的话也应该是25度
X=zeros(1,N);%保存各个采样点的真实温度
X_est=zeros(1,N);%各个时刻的估计值，有两类来源，初始的估计值是直接给出来的，后面的估计值是经过卡尔曼滤波得到的，是最接近真实值的；
X_pre=zeros(1,N);%各个时刻的预测值，也就是根据前面已经有的状态，结合状态转移矩阵得到的预测值。
Z=zeros(1,N);%各个时刻的测量值，也就是传感器给出的。
P_pre=0;%协方差矩阵有两类，一个是预测状态的协方差矩阵，另一个是估计状态的协方差矩阵，两者具有互推、递推关系。
P_est=zeros(1,N);%维数1

%初始化
X(1)=25.1;%初始的真实温度
P_est(1)=0.01;%初始值的协方差
Z(1)=24.9;%初始的测量值
X_est(1)=Z(1);%初始的最好值，估计值，其实不太好，直接用了初始测量值。
X_pre=Z(1);
%噪声
Q=0.01;%白噪声的方差,过程噪声，加入到了状态转移的过程，均值为0，方差为Q，衡量噪声的大小。
W=sqrt(Q)*(randn(1,N));
R=0.25;%测量噪声的方差，均值为0
V=sqrt(R)*(randn(1,N)-N/2);

%系统矩阵
A=1;%状态转移矩阵，理论方程应该是各个时刻温度恒定的；
G=1;%噪声驱动矩阵
H=1;%观测矩阵，即从状态空间到观测空间的映射。
E=eye(1);%系统1维

%模拟测量过程并滤波

for k=2:N
    X(k)=A*X(k-1)+G*W(k-1);%真实温度要考虑过程噪声
    %测量
    Z(k)=H*X(k)+V(k);
    
    %预测状态
    X_pre(k)=A*X_est(k-1);%依靠上一次的最优估计值
    %预测状态的协方差矩阵
    P_pre=A*P_est(k-1)*A'+Q;%过程噪声自然是干预到预测状态的协方差矩阵中
    
    K=P_pre/(H*P_pre*H'+R);%卡尔曼增益求解
    e=Z(k)-H*X_pre(k);%观测与预测的残差
    X_est(k)=X_pre(k)+e*K;%最优估计值
    P_est(k)=(E-K*H)*P_pre;
  
end
%数据可视化
t=1:N;   %同时画出('无干扰的理论值','直接测量值','实际值','kalman估计值')
figure
plot(t,X_expect,'-b',t,Z,'-r.',t,X_pre,'-ko',t,X_est,'-g*');
legend('无干扰的理论值','直接测量值','预测值','kalman估计值');
xlabel('采样时间');
ylabel('温度值/℃');

从头开始解释代码，首先设置才采样的步长（其实就是循环迭代的长度），设定N=120。

       首先假设房间里面的温度是25°，这个值是我们人为规定的，如果不受干扰，则房间里面的温度一直就是25°。但是我们都知道，房间又不是密封的，如果刮一点风，房间里面的温度就会发生变化。我们在房间里放置一个温度计，而这个温度计就是传感器来提供测量值，这个温度计也不是完全准确的，那我们怎么去知道房间里面的真实温度呢？其实是无法知道的，我们所能知道的知识它大概的一个值，就是估计值。而卡尔曼滤波的应用就是得到相对准确的估计值。

卡尔曼滤波是5个公式，包括两个预测和三个更新公式。接下来从代码入手，房间温度的系统方程为X(k)=A*X(k-1)+G*W(k-1)，因为是一阶的，我们可以设定A是一个1，W是一个具有正态分布的噪声序列，设定其方差为Q=0.01。同时，因为还有一个温度计，所以还会有一个测量方程Z(k)=H*X(k)+V(k)，当然，H也可以设定为1，V是一个测量误差，设定其方差为0.25。

那么现在开始解释卡尔曼滤波的五个公式，X_pre是一个估计值，是我们根据经验所知道的，它等与X_est，这个就是我们上一时刻的卡尔曼最优估计值。然后是预测的第二个公式，X这个状态量的方差就是P_pre(如果不知道什么是状态量，我建议去看自动控制原理中的现代控制理论中的状态方程），从公式中看到这个方差等于上一时刻最优方差加上过程噪声的方差。然后就是卡尔曼滤波的跟新阶段，首先求的是卡尔曼增益，K=P_pre/(H*P_pre*H'+R)，可以看到这个增益其实是与状态方差和测量噪声方差有关，而状态方差又和过程噪声的方差有关，所以卡尔曼增益是与Q和R这两个量有关的，我们可调节的也是这两个量。H是一个观测矩阵，设定为1 。下一个方程就是我们得到的卡尔曼的最优估计值，X_est(k)=X_pre(k)+e*K，这个e就是一个残差的简写，这个公式来理解就是我们根据经验的估计值乘上一个比例再加上卡尔曼增益乘上这个观测值的残差。从整个公式来理解就是使用上一次的最优结果预测当前的值，同时再用观测值对其进行修正，从而得到最优的结果。在得到新的卡尔曼估计值后，我们也要更新这个状态的方差用于下一次的迭代。这就是整个卡尔曼滤波的过程，讲解是我自己理解的，应该会很片面，多包涵，而且这只是最简单的一阶，高阶还须加强学习。