卡尔曼滤波的五大公式

预测：

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_;

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