卡尔曼滤波算法的matlab实现

卡尔曼滤波算法的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)

实验结果图:

相关推荐
落榜程序员17 分钟前
Java 基础-32-枚举-枚举的应用场景
java·开发语言
张彦峰ZYF18 分钟前
高频面试题(含笔试高频算法整理)基本总结回顾63
linux·运维·算法
晓131321 分钟前
第九章Python语言高阶加强-面向对象篇
java·开发语言
2301_7760452322 分钟前
什么是异步?
开发语言·区块链
alphaTao1 小时前
LeetCode 每日一题 2025/3/31-2025/4/6
算法·leetcode
Andrew_Ryan1 小时前
android use adb instsll cacerts
算法·架构
Wx120不知道取啥名2 小时前
C语言跳表(Skip List)算法:数据世界的“时光穿梭机”
c语言·数据结构·算法·list·跳表算法
禾小西3 小时前
Java 逐梦力扣之旅_[204. 计数质数]
java·算法·leetcode
LuckyLay3 小时前
LeetCode算法题(Go语言实现)_32
算法·leetcode·golang
ゞ 正在缓冲99%…3 小时前
leetcode295.数据流的中位数
java·数据结构·算法·leetcode·