卡尔曼滤波（非矩阵）在平衡直立车直立角度测量中的应用（MATLAB模拟仿真），简单易懂，详细注释，附卡尔曼滤波(矩阵)

   卡尔曼滤波（非矩阵）在平衡直立车直立角度测量中的应用（MATLAB模拟仿真），简单易懂，详细注释。

   前几天学习了一下卡尔曼滤波算法，感觉滤波效果很好，然后就尝试着运用卡尔曼滤波算法去解决一下实际问题，先从非矩阵的卡尔曼滤波算法开始，尝试着将他应用到 平衡直立车直立角度测量中，对陀螺仪测得的角速度和加速剂测得的加速度进行角度融合，本次仿真中我们用设立的模拟量代替（根据实际情况，陀螺仪的噪声干扰小，而加速度计的噪声干扰大，所以在设定参数的时候，我们让陀螺仪的值对最终结果影响大一些）

   不多说了，解释都放在注释里了，直接上代码，注释详细，简单易懂。

noise=randn(1,100);            %产生1*100的标准正态分布的随机矩阵，用来模拟噪声或者干扰
Z=15*ones(1,100)+10*noise;     %模拟观测量在数值15附近，波动幅值为10，可以看出这是一个比较大的干扰或者噪声
U=ones(1,100);                 %U用来模拟陀螺仪测得的角速度
Q=0.1;                         % Q，R ，X  P是一些参数，根据实际情况修改
R=8;                           %根据实际情况，陀螺仪的噪声干扰小，而加速度计的噪声干扰大，所以在设定这些参数的时候，陀螺仪的值比重要大一些
X=0;
P=1;
figure;
hold on;
plot([1,100],[15,15],'g')  %图中的绿色线，即设定的系统稳定值，此处为15，用来作参考

X_save=ones(1,100);        %将用来保存每次迭代的结果

for i=1:100
X_=X+0.8*U(i);             % X_为角度的预测值，X为上一次的角度，U为陀螺仪测得的角速度，此处为模拟值，物理上 X_=X+△t*U  
P_=P+Q;                    % P_为协方差预测值 Q表示预测模型的噪声，初值P(0)的设定需要根据系统效果来调试确定，初值一般在1附近
K=P_/(P_+R);               % K为卡尔曼系数（用来影响我们相信预测值多一点，还是观测值多一点），R为观测值噪声，R越大，K越小，那么观测值对最终角度的影响就会变小，起到抑制噪声的作用
X=X_+K*(Z(i)-X_);          % Z(i)为观测值，即为加速度计积分得到的角度，这个式子表示用观测值来修正预测值
P=(1-K)*P_;                % 修正协方差矩阵P，给下一轮迭代用 
plot(i,X,'b.');            % 画出每次迭代的结果
plot(i,Z(i),'r.');         %画出每次迭代的观测值（不经过卡尔曼滤波）

X_save(i)=X;               %用来保存每次迭代的结果，方便绘图
U(i+1)=15-X;               %模拟经过电机调控后的角速度   （因为是模拟，这个值是人为设置的，实际应用上是由陀螺仪测得的）
end

j=(1:100);                      %画图的横坐标
% plot(j,X_save,'b');
% plot(j,Z,'r');
x_interp=linspace(0,100,500);   %以下内容运用插值来画出图像，插值的目的是让曲线更平滑，更好看，并不是必要的，在实际应用中是不需要的
                                %linspace函数生成一个数组，数据的第一个元素值为0，
                                %最后一个元素为100，500是总采样点数，即在0到100之间取500个数，作为插值点
y=spline(j,X_save,x_interp);    %用spline函数对迭代结果进行三次样条插值
y2=spline(j,Z,x_interp);        %用spline函数对观测值进行三次样条插值
plot(x_interp,y,'b',x_interp,y2,'r');  %画出插值后的曲线

   运行结果：

 示例一

 示例二

 示例三

  由于数据是随机产生的，所以程序的每次运行图像都是不同的，图中绿色的线表示我们设定的在没有噪声干扰下的稳定值，用来作参考，红色的线是没有经过卡尔曼滤波的在大的噪声干扰下的观测值，蓝色的线是经过卡尔曼滤波后的值，经过对比我们可以发现卡尔曼滤波在大噪声干扰下的滤波效果是很好的。

   本文的主体部分到这就结束了，欢迎大家交流，互相学习，共同进步

   附：卡尔曼滤波（矩阵）

    以下内容参考neophack讲的卡尔曼滤波器的原理以及在matlab中的实现，个人感觉他讲的很好，简单易懂，对于想学习卡尔曼滤波原理的同学可以去看一下

     附链接：neophack讲的卡尔曼滤波器的原理以及在matlab中的实现视频链接

  进行一次实验，对100次迭代结果依次画出：

out=VideoWriter('kaerman2.avi');
out.FrameRate=2;
open(out);

    
z=(1:100);
noise=randn(1,100);
z=z+noise;

x=[0;0];
p=[1 0;0 1];
f=[1 1;0 1];
q=[0.0001,0; 0 0.0001];
h=[1 0];
r=1;


for i=1:100
    figure(i)
    axis([0 110 0.5 1.5]);
    axis manual
    hold on;
    plot([0,100],[1,1],'r')
    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_;
    plot(x(1),x(2),'b.','LineWidth',100);
    F=getframe(gcf);
    writeVideo(out,F);
    close all
end
close(out);

  结果视频展示（进行一次实验，对100次迭代结果依次画出）：

kaerman2

  附链接： 进行一次实验，对100次迭代结果依次画出视频超链接

  进行100次实验，对每次实验的结果依次画出：

out=VideoWriter('kaerman.avi');
out.FrameRate=10;
open(out);
for n=1:100
    
z=(1:100);
noise=randn(1,100);
z=z+noise;

x=[0;0];
p=[1 0;0 1];
f=[1 1;0 1];
q=[0.0001,0; 0 0.0001];
h=[1 0];
r=1;

figure(n);
axis([0 110 0 2]);
hold on;
axis manual
plot([1,100],[1,1],'r')
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_;
    
    plot(x(1),x(2),'b.');
end
  F=getframe(gcf);
  writeVideo(out,F);
  close all
end
close(out);

  结果视频展示（进行100次实验，对每次实验的结果依次画出）：

kaerman

  附链接： 进行100次实验，对每次实验的结果依次画出视频链接