卡尔曼滤波算法的matlab实现

卡尔曼滤波算法的matlab实现

figure;
hold on;

Z=(1:1:100);  %观测值：第一秒观测1m 第二秒观测两米  匀速运动, 每秒1m,  最后拟合的也是速度 1m/s

plot(Z);
plot([0,100], [1,1]);

noise=randn(1,100)*0.5;  %生成方差为1的高斯噪声
Z=Z+noise;  % 加入噪声

plot(Z);

X=[0;0];  %初始状态 随意设置; 经过几次迭代后，接近真实的状态
P=[1 0;0 1];  %状态协方差矩阵 ：随意设置;   经过几次迭代后，接近真实的状态
F=[1 1;0 1];  %状态转移矩阵, \Delta{t} 为 1, 每 秒采样一次
Q=[0.0001, 0;0 0.0001];  % 状态转移协方差矩阵：因为相信状态转移矩阵不会出错, 因此 Q方差 很小
H=[1 0];  % 观测矩阵
R=cov(noise);  % 观测矩阵协方差, 与加入的方差为1的高斯噪声相对应;

for i=1:100
    X_=F*X;  % 预测状态公式, 代码中缺乏控制量
    P_=F*P*F'+Q;
    K=P_*H'/(H*P_*H'+R);
    X=X_+K*(Z(i)-H*X_);
    P=(eye(2)-K*H)*P_;  % eye(2)是二阶单位阵
    
    % plot(X(1),X(2),'x');
    scatter(Z(i), X(1),'.');  % 画点, 横轴表示位置, 纵轴表示速度
    scatter(Z(i), X(2),'.');  % 画点, 横轴表示位置, 纵轴表示速度
    
end

%plot(Z, Z)

实验结果图：