卡尔曼滤波算法的matlab实现
figure;
hold on;Z=(1:1:100); %观测值：第一秒观测1m 第二秒观测两米 匀速运动, 每秒1m, 最后拟合的也是速度 1m/splot(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:100X_=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)
实验结果图：
