卡尔曼滤波实例 附matlab代码

本章有两个实例，看过关于卡尔曼滤波哔哩哔哩视频的应该知道DR_can，其中实例包括他第四节的excel转换成的matlab代码，还有一个温度的实例。具体的详细步骤在代码段有，不做过多说明。

【卡尔曼滤波器】5_直观理解与二维实例【包含完整的EXCEL代码】_哔哩哔哩_bilibiliKalman Filter 卡尔曼滤波器第5期。一个并不简单的简单例子。EXCEL程序下载链接：https://pan.baidu.com/s/1GdJe2eWIlaQrk2nrjemRCQ 提取码：txn3https://www.bilibili.com/video/BV1dV411B7ME?spm_id_from=333.999.0.0

卡尔曼滤波的五大公式

预测：

xhat_表示先验估计，xhat表示后验估计，带有_的先验估计，不带的后验估计

xhat_（k）=A*xhat(k-1)+B*u(k-1)

Pk_=A*Pk*A'+Q

更新

Kk=Pk_*H'*(H*Pk*H'_+R);

xhat（k）=xhat_（k）+Kk（Zk-H*xhat_（k））

Pk=(I-Kk*H)Pk_;

例题1：

clc
close all

%%首先需要实际值和测量值   需要考虑噪声信号（噪声信号符合高斯分布  期望为0）
delta_t=0.01;   
t=0:delta_t:30;
N = length(t);

A=[1 1;0 1];
H=eye(2);
x_real=zeros(2,N);
x_real(:,1)=[0;1];%首先给出一个相应的估计值
z=zeros(2,N);
w=zeros(2,N);
v=zeros(2,N);
%%噪声信号
for i=1:N
    w(:,i)=1*randn(2,1);
    v(:,i)=1*randn(2,1);
end
%%实际值
for i=2:N
    x_real(:,i)=A*x_real(:,i-1)+w(:,i);
end
%%观测值
for i=1:N
    z(:,i)=H*x_real(:,i)+v(:,i);
end

%%kalman_filter  根据上述得观测值来不断更新  
xhat=zeros(2,N);
xhat(:,1)=[0;1];%首先给出一个相应的估计值
xhat_=zeros(2,N);
Q=[0.1 0;0 0.1];
R=eye(2);
Pk=eye(2);
Pk_bar=zeros(2);
 for i=2:N
    xhat_(:,i)=A*xhat(:,i-1);
    Pk_bar=A*Pk*A'+Q;
    Kk=Pk_bar*H'*inv(H*Pk_bar*H'+R);
    xhat(:,i)= xhat_(:,i)+ Kk*(z(:,i)-H* xhat_(:,i));%最优估计值
    Pk=(eye(2)-Kk*H)*Pk_bar;
end

 

例题2：

%%温度估计卡尔曼

N=100;
dealt=0.01;
%%噪声信号
Q=0.001;%白噪声的方差,过程噪声，加入到了状态转移的过程，均值为0，方差为Q，衡量噪声的大小。
W=sqrt(Q)*randn(1,N);
R=0.25;%测量噪声的方差，均值为0
V=sqrt(R)*randn(1,N);

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

%%初始化
x_real=zeros(1,N);
x_real(1,1)=30;
z_obser=zeros(1,N);
z_obser(1)=29.6;

%%估计值和观测值
for i=2:N
    x_real(i)=A*x_real(i-1)+G*W(i);
    z_obser(i)=H* x_real(i)+V(i);
end

xhat_=0;
xhat=zeros(1,N);
% xhat(1)=x_real(1);估计值可以取实际值为初始化的值也可以使用观测值作为初始化的值，两个比较一下还是使用观测值
xhat(1)=z_obser(1);
Pk_bar=0;
Pk=zeros(1,N);
Pk(1)=0.001;%初始值的协方差
E=eye(1);
%%kalman
for i=2:N
   xhat_= A*xhat(i-1);
   Pk_bar=A*Pk(i-1)*A'+Q;
   K=Pk_bar*H'*inv(H*Pk_bar*H'+R);
   xhat(i)= xhat_+K*(z_obser(i)-H*xhat_);%%这个值是最有估计值
   Pk(i)=(E-K*H)*Pk_bar;
end
%误差计算：
error_mea=zeros(1,N);%%测量误差
error_kal=zeros(1,N);%%kalman误差

for k=1:N
    error_mea(k)=abs(z_obser(k)-x_real(k));%测量值和实际值的真实误差
    error_kal(k)=abs(xhat(k)-x_real(k));
end
error_mea;
error_kal;
%%绘图
t=1:N;
figure
plot(t,error_mea,'-b.',t,error_kal,'-r');
xlabel('t');
ylabel('error');
legend('error_mea','error_kal');