卡尔曼滤波原理及应用仿真（2）：线性卡尔曼滤波

1. 最优滤波或最优估计是指在最小方差意义下的最优滤波或估计，即要求信号或状态的最优估计值与相应的真实值的误差的方差最小。卡尔曼滤波是一种时域滤波方法，采用状态空间方法描述系统，算法采用递推形式，数据存储量少，不仅可以处理平稳随机过程，也可以处理多维和非平稳随机过程。

2. 扩展卡尔曼滤波（EKF）是一种应用广泛的非线性系统滤波方法，将非线性系统一阶线性化，利用标准卡尔曼滤波估计；无迹卡尔曼滤波·（UKF），以UT变换为基础，采用卡尔曼线性滤波的框架，摒弃对非线性函数进行线性化的传统做法，然后对于一步预测方程，使用UT变换来处理均值和协方差的非线性传递。

3. Kalman滤波器
   状态空间模型描述的动态系统：
   
   Kalman滤波器：
   

4. Kalman滤波在船舶GPS导航定位系统中的应用
   
   
   

5. MATLAB仿真

   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %功能说明：kalman滤波在船舶GPS导航定位系统中的应用
   function gps_kf
   clc;clear;
   T=1;
   N=80/T;
   X=zeros(4,N);
   X(:,1)=[-100,2,200,20];
   Z=zeros(2,N);
   Z(:,1)=[X(1,1),X(3,1)];
   delta_w=1e-2;
   Q=delta_w*diag([0.5,1,0.5,1]);
   R=100*eye(2);
   F=[1,T,0,0;0,1,0,0;0,0,1,T;0,0,0,1];
   H=[1,0,0,0;0,0,1,0];
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   for t=2:N
   X(:,t)=F*X(:,t-1)+sqrtm(Q)*randn(4,1);
   Z(:,t)=H*X(:,t)+sqrtm(R)*randn(2,1);
   end
   %kalman滤波
   Xkf=zeros(4,N);
   Xkf(:,1)=X(:,1);
   P0=eye(4);
   for i=2:N;
   Xn=F*Xkf(:,i-1);
   P1=F*P0*F'+Q;
   K=P1*H'*inv(H*P1*H'+R);
   Xkf(:,i)=Xn+K*(Z(:,i)-H*Xn);
   P0=(eye(4)-K*H)*P1;
   end
   %误差分析
   for i=1:N
   Err_Observation(i)=RMS(X(:,i),Z(:,i));
   Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));
   end
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %画图
   figure
   hold on;box on;
   plot(X(1,:),X(3,:),'-k');
   plot(Z(1,:),Z(2,:),'-b.');
   plot(Xkf(1,:),Xkf(3,:),'-r+');
   legend('真实轨迹','观测轨迹','滤波轨迹');
   xlabel('横坐标X/m');
   ylabel('纵坐标Y/m');
   
   figure
   hold on;box on;
   plot(Err_Observation,'-ko','MarkerFace','g')
   plot(Err_KalmanFilter,'-ks','MarkerFace','r')
   legend('滤波前误差','滤波后误差')
   xlabel('观测时间/s');
   ylabel('误差值')；
   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   %计算欧氏距离子函数
   function dist=RMS(X1,X2
   if length(X2)<=2
    dist=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(2))^2);
    else
    dist=sqrt((X1(1)-X2(1))^2+(X1(3)-X2(3))^2);
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   

仿真结果

观测轨迹在明显震荡，噪声观测的影响较大，经过Kalman滤波后，轨迹更加接近目标的真实运动轨迹。

滤波后的误差大幅度减小，Kalman滤波大限度地降低噪声的影响。