北斗定位|经典卡尔曼滤波算法原理以及matlab仿真|Kalman|一维/四维应用场景仿真

一 、卡尔曼滤波基本步骤

卡尔曼滤波的状态方程和量测方程为：

 

卡尔曼滤波只是根据前一个估计值与最近一个观测数据来估计信号的当前值，它是用状态方程和递推方法进行估计的，适用于线性系统。卡尔曼滤波依据最小方差原则估计，关于它的原理网上很多，也可以参考《随机信号处_陆光华》这本书，讲得非常详细。传统卡尔曼滤波算法的具体步骤为：

（1）初始化，初始化状态变量和协方差矩阵初值，若知道状态的初始统计特性，那么初始条件可以设为：

 （2） 对系统方程进行状态一步预测和协方差一步预测。（注意：这里一步预测得到的关于状态变量x的估计值并不是当前时刻的最优估计，而是一种次优估计，因为一步预测没有考虑到噪声的情况，是存在误差的。因此，后面需要利用滤波增益矩阵对它进行类似于补偿校正的过程，即状态更新。）

（3） 计算增益矩阵K，从而进行状态更新和协方差更新：

 

以上便是卡尔曼滤波的一个计算周期。

二 卡尔曼滤波matlab仿真

2.1 一维卡尔曼滤波的应用仿真

应用场景：电力工程师激光测距仪对架空线的高度进行测量，假定激光测距仪采样时间为10ms，在1s内能采样100次，那么对这100次数据进行滤波，便能稳定输出一个比较准确的数值。

架空线的高度可能受到风的吹动或者飞鸟对路线的干扰等扰动，我们将这个扰动称为过程噪声W(k),方差为Q,假定Q=0.0001。状态变量X(k)便是第k次采样时的高度。

而用激光测距仪对高度进行测量过程中存在测距误差，即存在测量噪声V(k),设其方差为R=0.0025。

因此，由以上描述可知，确定该系统的状态方程和观测方程为：

采用matlab编写程序仿真如下： 

% 一维卡尔曼滤波仿真
% 初始化
   Q=0.0001;  % 过程噪声方差
   R=0.0025;  %量测噪声方差
   x0=8.01;
   P0=0.0004;

% 系统模型建立
F=1;  % 状态转移矩阵
G=1;  % 噪声驱动矩阵
H=1;  % 量测矩阵
T=10*1e-3; % 采样时间
t_total=1; % 仿真总时间
step=100;  % 仿真总步数（相当于时间采样点数）
W=sqrt(Q).*randn(1,step); % 各时刻的过程噪声
V=sqrt(R).*randn(1,step); % 各时刻的观测噪声

% 开始仿真目标的状态和测量过程
X=zeros(1,step);X(1,1)=8.00; % 目标真实值的状态初始化
Z=zeros(1,step);Z(1,1)=8.01;
Xkf=zeros(1,step);Xkf(1,1)=Z(1,1); % 卡尔曼滤波得到的状态初始化
P=zeros(1,step);P(1,1)=0.0001; % 协方差初始化

for k=2:step
    % 首先是被测量目标一侧的信息
    % X时=是架空线的真实高度值，它由真实值和风吹扰动导致的干扰组成
    % X是计算机模拟仿真，是真实状态的模拟，测距仪是永远测不到的
    X(1,k)=F*X(1,k-1)+G*W(1,k-1); % 状态方程
    % 观测站一侧的信息
    Z(1,k)=H*X(1,k)+V(1,k); % 观测方程
    Xpre=F*Xkf(1,k-1); % 状态的一步预测
    Ppre=F*P(1,k-1)*F'+Q; % 协方差预测
    K=Ppre*H'*inv(H*Ppre*H'+R); % 计算滤波增益矩阵
    ek=Z(1,k)-H*Xpre; % 计算新息
    Xkf(1,k)=Xpre+K*ek; % 状态更新
    P(1,k)=(1-K*H)*Ppre; % 协方差 更新
end

% 误差分析
MeasuredDeviation=abs(Z-X); % 观测值与真实值之间的误差
FilterResultDeviation=abs(Xkf-X);% 滤波值与真实值之间误差

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 画图
% 画轨迹图
figure(1);
hold on;
box on;
axis([0 step 7.5 8.5]) % 坐标轴范围
xlabel('time/10ms');ylabel('messure value');
plot(1:step,X(1,1:step),'-k'); % 真实值
plot(1:step,Z(1,1:step),'-ko','MarkerFace','g'); % 测量值
plot(1:step,Xkf(1,1:step),'-r'); % 滤波值
legend('真实值','测量值','滤波值');

% 误差分析图
figure(2);
hold on;
box on;
% axis([0 step 7.5 8.5])
xlabel('time/10ms');ylabel('误差值');
plot(MeasuredDeviation,'-r','MarkerFace','g'); %测量误差
plot(FilterResultDeviation,'-ko','MarkerFace','g'); %滤波结果误差
legend('测量误差','滤波误差');

2.2卡尔曼滤波用于自由落体运动目标的跟踪问题

% 卡尔曼滤波用于自由落体运动目标的跟踪问题
% 系统信息初始化
F=[1,1;0,1]; % 状态转移矩阵
B=[0.5;1]; % 控制量
G=1;
H=[1,0];% 观测矩阵
step=1000; % 仿真时间，即时间序列总数（离散的）
I=eye(2); %单位矩阵，二维系统

% 噪声
U=-10*ones(1,step); %控制量
Q=0;  % 过程噪声方差为0，即下落过程忽略空气阻力
W=sqrt(Q)*randn(2,step); %过程噪声，也为0
R=1; % 观测噪声
V=sqrt(R)*randn(1,step);% 观测噪声

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 初始化
X=zeros(2,step); %物体真实状态
X(:,1)=[95 1]'; % 初始位移以及速度
P0=[10,0;0,1]; %初始化协方差
Z=zeros(1,step); %观测量
Z(1)=H*X(:,1); %初始观测值
Xkf=zeros(2,step);  %    
Xkf(:,1)=X(:,1); %卡尔曼估计状态初始化
     
% 位移和速度的均方误差
Pmse=zeros(2,step);
Pmse(1,1)=P0(1,1);
Pmse(2,1)=P0(2,2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=2:step
    % 物体下落，受到状态方程的驱动（假设忽略空气阻力），真实值
    X(:,k)=F*X(:,k-1)+B*U(1,k-1)+W(k);

    % 位移传感器对目标进行观测
    Z(k)=H*X(:,k)+V(k);
    % 得到观测信息后开始卡尔曼滤波
    Xpre=F*Xkf(:,k-1)+B*U(k);% 第一步。状态预测
    Ppre=F*P0*F'+Q;  % 协方差预测
    K=Ppre*H'*inv(H*Ppre*H'+R);
    ek=Z(k)-H*Xpre;

    Xkf(:,k)=Xpre+K*ek;
    P0=(I-K*H)*Ppre;  % 协方差更新
    % 误差均方值
    Pmse(1,k)=P0(1,1); % 保存位移均方误差
    Pmse(2,k)=P0(2,2); % 速度均方误差
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 误差计算
messure_err_x=zeros(1,step); %位移的测量误差
kalman_err_x=zeros(1,step); % 卡尔曼估计的位移与真实位移之间的偏差
kalman_err_v=zeros(1,step);  % 速度

for k=1:step
    messure_err_x(k)=Z(k)-X(1,k); %位移的测量误差
    kalman_err_x(k)=Xkf(1,k)-X(1,k); % 卡尔曼估计的位移与真实位移之间的偏差
    kalman_err_v(k)=Xkf(2,k)-X(2,k);  % 速度
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 画图输出
figure(1);
plot(V);
title('观测噪声');

figure(2);
hold on;
box on;
plot(messure_err_x,'-r'); % 测量的位移误差
plot(kalman_err_x,'-b'); % 卡尔曼滤波位置误差
legend('测量误差','卡尔曼估计误差');

figure(3);
plot(kalman_err_v);
title('速度偏差');

figure(4);
plot(Pmse(1,:));
title('位移误差均方值');

figure(5);
plot(Pmse(2,:));
title('速度误差均方值');

2.3 卡尔曼滤波在GPS导航定位系统中的应用

% 参数初始化
dt=1; % 雷达扫描周期（也就是采样周期）
step=80/dt; % 总的采样次数
F=[1,dt,0,0;0,1,0,0;0,0,1,dt;0,0,0,1]; %状态转移矩阵
H=[1,0,0,0;0,0,1,0];% 观测矩阵
delta_w=1e-2; %如果增大这个参数，目标真实轨迹就是曲线了
Q=delta_w*diag([0.5,1,0.5,1]);
W=sqrt(Q)*randn(4,step);% 过程噪声
R=10*eye(2);     % 观测噪声方差
V=sqrt(R)*randn(2,step); %观测噪声

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
X=zeros(4,step); % 存放目标的真实位置和速度，四维的
X(:,1)=[-100;2;200;20]; %目标初始的位置和速度
Z=zeros(2,step); %传感器对位置的观测
Z(:,1)=[X(1,1) X(3,1)]; % 观测量初始化
Xkf=zeros(4,step);
Xkf(:,1)=X(:,1); %滤波状态初始化
P=eye(4); % 一个单位矩阵,协方差矩阵初始化
I=eye(4);
for k=2:step
     % 船舶运动真实值
    X(:,k)=F*X(:,k-1)+W(:,k); % 目标真实轨迹

    % 获取卫星数据，观测信息开始滤波
    Z(:,k)=H*X(:,k)+V(:,k); % 自导航，观测信息
    % 得到观测信息后开始卡尔曼滤波
    Xpre=F*Xkf(:,k-1);% 第一步。状态预测
    Ppre=F*P*F'+Q;  % 协方差预测
    K=Ppre*H'*inv(H*Ppre*H'+R);
    ek=Z(:,k)-H*Xpre;

    Xkf(:,k)=Xpre+K*ek;
    P=(I-K*H)*Ppre;  % 协方差更新
end 

    % 误差分析
    for i=1:step
        Err_Observation(i)=RMS(X(:,i),Z(:,i));
        Err_KalmanFilter(i)=RMS(X(:,i),Xkf(:,i));
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % 画图
    figure(1);
    hold on;
    box on;
    plot(X(1,:),X(3,:),'-k');
    plot(Xkf(1,:),Xkf(3,:),'-r+');
    plot(Z(1,:),Z(2,:),'-b.');
    legend('真实轨迹','滤波轨迹','观测轨迹');

    figure(2);
    hold on;
    box on;
    xlabel('时间/s');
    ylabel('跟踪误差');
    plot(Err_Observation,'-ko','MarkerFace','g');
    plot(Err_KalmanFilter,'-ks','MarkerFace','r');
    legend('观测偏差','滤波后偏差');


    % 子函数 计算偏差
    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
end

 参考书籍：《目标定位跟踪原理以及应用》