从零开始码出卡尔曼滤波代码 |
您所在的位置：网站首页 › 忠厚老实的老王作文 › 从零开始码出卡尔曼滤波代码 |
从零开始码出卡尔曼滤波代码
|
文章目录
@[toc]
tips目标：生成一段随机信号，并滤波1 生成数据，以及对应的观测的数据
2 滤波实现一 粗糙的实现2.1 观测方程：当前的状态+噪声2.2 预测方程实现：最粗糙的建模 当前的状态=上一步的状态+噪声2.3 初始步：给一个初值（只需要做一次）2.2.2 更新步和预测步：其实就是算矩阵形式的卡尔曼的5条公式：
2.4 完整代码
3 滤波实现二3.0 扩展一下状态：x====>[x,x',x'']3.1 观测方程：当前的状态+噪声,跟上个例子的一样3.2 预测方程实现：状态扩展成[x,x',x'']，当前状态是上一个状态和状态的导数（速度）以及二阶导数（加速度）的多项式3.2 预测和更新：
4 两个传感器问题
tips
用的融合方法被称为“并行滤波融合”（又称观测值扩维融合），是集中式融合系统中的一种方法。集中式融合系统还包含两种方法：序贯滤波融合（又称顺序滤波融合）和数据压缩滤波融合（又称观测值加权融合）。与集中式融合系统对应的还有分布式融合系统。 更多关于多传感器数据融合的内容，大家可参考韩崇昭老师的《多源信息融合》第二版。 目标：生成一段随机信号，并滤波这里只是对数据进行滤波，并没有进行姿态什么的计算。 1 生成数据，以及对应的观测的数据 t=0.1:0.01:1; L=length(t); %生成真实信号x，以及观测y %首先初始化 x=zeros(1,L); y=x; y2=x; %生成信号，设x=t^2 for i=1:L x(i)=t(i)^2; y(i)=x(i)+normrnd(0,0.1);%正态分布，参数为期望和标准差 y2(i)=x(i)+normrnd(0,0.1); end %%%%%%%%%%%信号生成完毕%%%% plot(t,x,t,y,'LineWidth',2);真实数据和观测数据的图示：
滤波效果图示：看起来滤波的效果只有一点点，原因是模型建得太粗糙了，Q太大了，这就意味着更多是相信观测值。
如果我们将Q的值改为0.01的效果：稍微好一点，但是还是不好，预测的模型太粗糙了。 观测一下，真实数据是时间的平方，预测模型是直接线性增长，所以在t比较小的时候，也就是 前面那一段，滤波出来的值是比较靠近真实值的。后面，t的平方快速增长，导致差异越来越大。 多项式信号多求几阶导数，总会比较平缓 此时，状态是[x,x’,x’'] % X(K)=X(K-1)+X'(K-1)*dt+X''(K-1)*dt^2*(1/2!)+Q2 X'(K)=0*X(K-1)+X'(K-1)+X''(K-1)*dt+Q3 X''(K)=0*X(K-1)+0*X'(K-1)+X''(K-1)+Q4 将上面的表达式代入状态方程x_k = F *x_k-1+Q，提取得到F： F= [1 dt 0.5*dt^2 0 1 dt 0 0 1 ] 协方差 Q= [Q2 0 0 0 Q3 0 0 0 Q4] 这种协方差表示噪声不相关。 3.2 预测和更新：
跟第一次建模对比，效果好很多了。 我们也可以使用傅里叶变换消除噪声，再傅里叶逆变换回来，可能可以达到同样的效果，但是，贝叶斯滤波有它的优点，可以进行在线滤波，实时滤波 ,因为贝叶斯滤波是递推的，而傅里叶则不行。 ##3.3 完整代码 t=0.1:0.01:1; L=length(t); %生成真实信号x，以及观测y %首先初始化 x=zeros(1,L); y=x; y2=x; %生成信号，设x=t^2 for i=1:L x(i)=t(i)^2; y(i)=x(i)+normrnd(0,0.1);%正态分布，参数为期望和标准差 y2(i)=x(i)+normrnd(0,0.1); end %plot(t,x,t,y,'LineWidth',2); %%%%%%%%%%%信号生成完毕%%%% %X(K)=X(K-1)+X'(K-1)*dt+X''(K-1)*dt^2*(1/2!)+Q2 %Y(K)=X(K)+R R~N(0,1) %此时状态变量X=[X(K) X'(K) X''(K) ]T(列向量） %Y(K)=H*X+R H=[1 0 0](行向量） %预测方程 %X(K)=X(K-1)+X'(K-1)*dt+X''(K-1)*dt^2*(1/2!)+Q2 %X'(K)=0*X(K-1)+X'(K-1)+X''(K-1)*dt+Q3 %X''(K)=0*X(K-1)+0*X'(K-1)+X''(K-1)+Q4 %%多项式信号多求几阶导数，总会比较平缓，而 %X''(K)=X''(K-1)+Q3正是描述平缓的随机过程，这种建模相对精细一些，适用范围也较广 %F= 1 dt 0.5*dt^2 % 0 1 dt % 0 0 1 %H=[1 0 0] %Q= Q2 0 0 % 0 Q3 0 % 0 0 Q4 协方差矩阵 dt=t(2)-t(1); F2=[1,dt,0.5*dt^2;0,1,dt;0,0,1];%%%此处要注意矩阵是否病态，若dt特别小，易导致矩阵病态或精度丢失 H2=[1,0,0]; Q2=[1, 0, 0;0, 0.01, 0;0, 0, 0.0001]; R2=3; %%%设置初值%%%% Xplus2=zeros(3,L); Xplus2(1,1)=0.1^2; Xplus2(2,1)=0; Xplus2(3,1)=0; Pplus2=[0.01, 0, 0;0, 0.01,0;0, 0, 0.0001]; for i=2:L %这里没有更新dt,正常来说，dt,F也要更新的。 %%%预测步%%% Xminus2=F2*Xplus2(:,i-1); Pminus2=F2*Pplus2*F2'+Q2; %%%更新步%%%% K2=(Pminus2*H2')*inv(H2*Pminus2*H2'+R2); Xplus2(:,i)=Xminus2+K2*(y(i)-H2*Xminus2); Pplus2=(eye(3)-K2*H2)*Pminus2; end plot(t,x,'r',t,y,'g',t,Xplus2(1,:),'b','LineWidth',2); 4 两个传感器问题生成真实数据以及两个传感器的观测数据 t=0.1:0.01:1; L=length(t); x=zeros(1,L); y=x; y2=x; %生成信号，设x=t^2 for i=1:L x(i)=t(i)^2; y(i)=x(i)+normrnd(0,0.1);% 传感器1 y2(i)=x(i)+normrnd(0,0.1);%传感器2 end状态方程和观测方程采用上面模型2的方式 X(K)=X(K-1)+X’(K-1)dt+X’‘(K-1)dt^2(1/2!)+Q2 X’(K)=0X(K-1)+X’(K-1)+X’‘(K-1)dt+Q3 X’'(K)=0X(K-1)+0*X’(K-1)+X’'(K-1)+Q4 %%多项式信号多求几阶导数，总会比较平缓，而 Y1(K)=X(K)+R1 Y2(K)=X(K)+R2 F3=[1, dt, 0.5*dt^2;0, 1, dt;0, 0, 1]; H3=[1,0,0;1,0,0];%两个传感器，所以H3比H2大一倍 Q3=[1, 0, 0;0, 0.01, 0;0, 0, 0.0001]; R1 = 3； R2 =3； R3=[R1,0;0,R2];% 这里跟上面模型二不同是有了2个传感器，所以是协方差矩阵%%%设置初值%%%% Xplus3=zeros(3,L); Xplus3(1,1)=0.1^2; Xplus3(2,1)=0; Xplus3(3,1)=0; Pplus3=[0.01, 0,0;0, 0.01,0;0,0, 0.0001];更新和预测 for i=2:L %%%预测步%%% Xminus3=F3*Xplus3(:,i-1); Pminus3=F3*Pplus3*F3'+Q3; %%%更新步%%%% K3=(Pminus3*H3')*inv(H3*Pminus3*H3'+R3); Y=zeros(2,1); Y(1,1)= y(i); Y(2,1)=y2(i); Xplus3(:,i)=Xminus3+K3*(Y-H3*Xminus3); Pplus3=(eye(3)-K3*H3)*Pminus3; end plot(t,x,'r',t,y,'g',t,Xplus3(1,:),'b','LineWidth',2);
|
实际的时候，我们不知道蓝色代表的真实值，只有红色的观测值
如果将R=0.01，Q=1，可以看到，滤波的值和观测的值重合了，表示完全相信观测值。 
R,Q,初值改得离谱一点： Xplus1(1)=1；%真实初值的100倍。 可见，卡尔曼对初值不敏感，跟初值是0.01差不多。 
将观测噪声改大10倍：y2(i)=x(i)+normrnd(0,1); 
【本文地址】