卡尔曼滤波器学习笔记

最新推荐文章于 2022-05-16 17:20:34 发布

Yao_Magic

最新推荐文章于 2022-05-16 17:20:34 发布

阅读量433

点赞数

文章标签： Kalman滤波

本文链接：https://blog.csdn.net/sinat_33546981/article/details/89004488

版权

卡尔曼滤波器具有两个部分的信息更新过程：时间更新过程和观测更新过程。
考虑如下的状态空间模型描述的系统：
在这里插入图片描述
其核心公式为：

对状态做一步预测：
一步预测协方差阵：
求滤波增益矩阵：
状态更新：
协方差更新：

一维Kalman滤波器Matlab仿真:

function kalmanfilter1D
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%状态方程：x(k)=Fx(k-1)+GW(k)
%测量方程：z(k)=Hx(K)+v(k)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all;clear;
%一维Kalman滤波应用仿真实例
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
F=1;%状态转移矩阵
G=1;%噪声驱动矩阵
H=1;%观测矩阵
Q=0.0001;%过程噪声方差
R=0.0025;%观测噪声方差
T=100;%10ms
W=sqrt(Q)*randn(1,T);%随机过程噪声
V=sqrt(R)*randn(1,T);%随机测量噪声
I=eye(1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
X=zeros(1,T);
X(1,1)=10.00;%状态初始化
Z=zeros(1,T);
Z(1,1)=10.01;%观测值初始化
Xkf=zeros(1,T);
Xkf(1,1)=Z(1,1);%Kalman滤波器状态初始化
P=zeros(1,T);
P(1,1)=0.0001;%协方差初始化
for k=2:T
    X(1,k)=F*X(1,k-1)+G*W(1,k);%状态方程
    Z(1,k)=H*X(1,k)+V(1,k);%观测方程
    %%%%%状态预测%%%%%%%%%%%
    Xpre=H*Xkf(1,k-1);
    %%%%%协方差预测%%%%%%%%%
    Ppre=F*P(k-1)*F'+Q;
    %%%%%计算卡尔曼增益%%%%%
    K=Ppre*inv(H*Ppre*H'+R);
    e=Z(k)-H*Xpre; %计算新息
    %%%%%状态更新%%%%%%%%%%%
    Xkf(1,k)=Xpre+K*e;
    %%%%%协方差更新%%%%%%%%%
    P(1,k)=(I-K*H)*Ppre;
end
%画图
figure
hold on;axis([0 T 9.5 10.5])
plot(1:T,X(1,1:k),'-k');%真实值
plot(1:T,Z(1,1:k),'-ko');%测量值
plot(1:T,Xkf(1,1:k),'-r.');%滤波值

仿真结果：
在这里插入图片描述