卡尔曼滤波的简单应用（一）

最新推荐文章于 2023-07-04 11:17:05 发布

原创

最新推荐文章于 2023-07-04 11:17:05 发布 · 2.5k 阅读

收录于

这篇博客不深入理论，直接展示卡尔曼滤波的代码应用，提供了C++/C/MATLAB的实现，适合希望快速了解和实践卡尔曼滤波的读者。

这里不打算讲理论，直接附上代码，理论具体可以参见【1】【2】

%功能：卡尔曼滤波程序
%
clear all, clc
N=2000;  
W(1)=0;                     %W为过程的噪声(高斯白噪声)
W=randn(1,N);                
X(1)=25;                    %房间的真实温度为25度
A=1;                        %A为方程中A(k)
for k=2:N;  
    X(k)=A*X(k-1)+W(k-1);   %X(k)是k时候的系统状态，经验得到
end  
  
V=randn(1,N);               %V为测量的噪声
q1=std(V);  
Rvv=q1.^2;                  %一维时，协方差退化为方差；
R=Rvv;                      %V的方差
R=1e-1;
q2=std(X);  
Rxx=q2.^2;                  %X的方差   
q3=std(W);  
Rww=q3.^2;                  
Q=Rww;                      %W的方差:
Q=1e-6;
H=1;                        %H为方程中H(k),H是测量系统的参数
Z=H*X+V;                    %Z为测量值，如使用温度计测量得到
  
P(1)=10;                     %P(0|0)
s(1)=1;                     %X(0|0)房间的真实温度为25度
for k=2:N;  
    P1(k)=A*P(k-1)*A'+Q;                   %P1为当前预测结果的协方差
    Kg(k)=P1(k)*H'/(H*P1(k)*H'+R);         %Kg:卡尔曼增益（Kalman