这个卡尔曼滤波程序哪位大哥可以帮我解释一下?

这个卡尔曼滤波程序哪位大哥可以帮我解释一下?,第1张

clear

N=200%取200个数

w(1)=0

w=randn(1,N)%产生一个1×N的行向量,第一个数为0,w为过程噪声(其和后边的v在卡尔曼理论里均为高斯白噪声)

x(1)=0%状态x初始值

a=1%a为状态转移阵,此程序简单起见取1

for k=2:N

x(k)=a*x(k-1)+w(k-1) %系统状态方程,k时迅颤皮刻的状态等于k-1时刻状态乘以状态转移阵加噪声(此处忽略了系统的控制量)

end

V=randn(1,N)%测量噪声

q1=std(V)

Rvv=q1.^2

q2=std(x)

Rxx=q2.^2%此方程未用到Rxx

q3=std(w)

Rww=q3.^2%Rvv、Rww分别为过程噪声和测量洞山噪声的协方差(此方程只取一组数方差与协方差相同)

c=0.2

Y=c*x+V%量测方差,c为量测矩阵,同a简亩差化取为一个数

p(1)=0%初始最优化估计协方差

s(1)=0%s(1)表示为初始最优化估计

for t=2:N

p1(t)=a.^2*p(t-1)+Rww%p1为一步估计的协方差,此式从t-1时刻最优化估计s的协方差得到t-1时刻到t时刻一步估计的协方差

b(t)=c*p1(t)/(c.^2*p1(t)+Rvv)%b为卡尔曼增益,其意义表示为状态误差的协方差与量测误差的协方差之比(个人见解)

s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1))%Y(t)-a*c*s(t-1)称之为新息,是观测值与一步估计得到的观测值之差,此式由上一时刻状态的最优化估计s(t-1)得到当前时刻的最优化估计s(t)

p(t)=p1(t)-c*b(t)*p1(t)%此式由一步估计的协方差得到此时刻最优化估计的协方差

end

t=1:N

plot(t,s,'r',t,Y,'g',t,x,'b')%作图,红色为卡尔曼滤波,绿色为量测,蓝色为状态

%整体来说,此卡尔曼程序就是一个循环迭代的过程,给出初始的状态x和协方差p,得到下一时刻的x和p,循环带入可得到一系列的最优的状态估计值,此方法通常用于目标跟踪和定位。

%本人研究方向与此有关,有兴趣可以交流下。

%% 连续空间系统状态方升简桐程

% X_dot(t) = A(t)*X(t) + B(t)*W(t)

%% 转化为离散时间系统状态方程

% X(k+1) = F*X(k) + G*W(k)

T = 0.1% T为采样吵坦咐森时间间隔

[F,G]=c2d(A,B,T)


欢迎分享,转载请注明来源:内存溢出

原文地址: http://outofmemory.cn/yw/12473056.html

(0)
打赏 微信扫一扫 微信扫一扫 支付宝扫一扫 支付宝扫一扫
上一篇 2023-05-25
下一篇 2023-05-25

发表评论

登录后才能评论

评论列表(0条)

保存