惯性导航学习过程记录（2d导航+EKF）

1.一个2d导航的EKF案例：

        （个人理解）例如，有一个小车，我想知道它的位置、速度、航向角的信息，车身上有编码器和陀螺仪，我能从中得到来自于这个传感器的速度和航向速率信息，但是传感器误差大，利用这两个信息根据运动方程计算出的位置信息不准；另一种手段是利用GPS直接获取位置信息，但GPS也不一定准。因此可以利用卡尔曼滤波的方法，综合考虑两种传感器的可信度，得到一个综合的状态估计结果。

        首先，我想获取位置、速度、航向信息，因此把他们设为状态向量X, x =  [ 位置x，位置y,   航向角yaw， 速度v,], 其次把从车身上获取到的编码器和陀螺仪信息设为控制向量u，u =  [ 速度v  ，航向速率yawrate] ，GPS只能观测到位置信息，因此将位置设为观测向量Z。

---

 1.1 列运动方程

例如运动状态为匀速运动但会持续偏航，则下一时刻速度不变，下一时刻航向角受偏航率影响，位置同时受速度和偏航率影响，则运动方程如下：

下一时刻： pos_x  =  pos_x + cos(yaw)*v*dt

                   pos_y  =  pos_y + sin(yaw)*v*dt

                   yaw     =  yaw + yawrate*dt

                    v       =   v       匀速，速度不变

---

1.2 列状态方程

根据上述运动方程，列出状态方程，即改写成x = F*x + B*u的形式.

---

1.3  列观测模型

这个很简单，状态向量中就前两个能观测到。

 上面是真实位置，但是GPS会有观测误差，因此在仿真时，制作GPS的观测值需要在真实轨迹的基础上，把GPS的噪声加进去。

% 添加测量噪声
GPS_NOISE = diag([0.7, 0.7]) ^(2);
% 观测模型z = H * x;
z = observation_model(x_gt) + GPS_NOISE* randn(2,1);

 同理，编码器和陀螺仪也有噪声，因此仿真时，利用状态方程进行航迹推算时，输入的u需要额外考虑噪声。

% 添加传感器噪声
global INPUT_NOISE;
u_dr = u + INPUT_NOISE * randn(2,1);

% 生成真实DR轨迹
x_dr = motion_model(x_dr, u_dr, dt);

---

1.4 EKF滤波环节

EKF 需要用到的输入： 传感器测量值，GPS观测值，过程噪声矩阵，量测噪声矩阵。

过程噪声矩阵，量测噪声矩阵的值自己设置，我会在后面比较该值设置对最终导航结果的影响。

%过程噪声
Q = diag([  %预测状态协方差
          0.1  % x轴上的位置方差
          0.1  % y轴上位置方差
         deg2rad(1.0)  % 航向角方差
         1.0 % 速度方差
         ])^(2);

%量测噪声
R = diag([
           3 % GPS X量测噪声
           3 % GPS Y量测噪声
            ])^(2);

EKF就是更新和预测两个环节，循环往复，该部分代码照抄就可以，原理参考博主：扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）_ekf卡尔曼滤波实例_O_MMMM_O的博客-CSDN博客

比较难的地方是里面需要雅克比矩阵J，这个需要根据状态方程去推导，这里记录一下中JA推导过程:

计算原则是下面这样：

 自己的运动方程：

pos_x  =  pos_x + cos(yaw)*v*dt

pos_y  =  pos_y + sin(yaw)*v*dt

yaw     =  yaw + yawrate*dt

 v       =   v 

 把自己的运动方程套进计算原则中：

第一行： pos_x + cos(yaw)*v*dt     逐个对pos_x ，pos_y，yaw， v  求导

得到： [1      0    -dt*v*sin(yaw),     dt*cos(yaw)]

第二行： pos_y + sin(yaw)*v*dt       逐个对pos_x ，pos_y，yaw， v  求导

得到：[ 0      1     dt*v*cos(yaw)     dt*sin(yaw)]   

以此类推，得到所求的J：

J = [1      0    -dt*v*sin(yaw),     dt*cos(yaw); 
       0      1     dt*v*cos(yaw)     dt*sin(yaw); 
       0      0         1                           0; 
       0      0         0                           1];

 EKF 需要计算两个两个雅克比矩阵，另一个计算方法一样。

---

2. 完整程序

clear;
clc
close all;

%% EKF 2D 定位
% 参考: https://pythonrobotics.readthedocs.io/en/latest/modules/localization.html#histogram-filter-localization

%过程噪声
Q = diag([  %预测状态协方差
         0.1  % x轴上的位置方差
          0.1  % y轴上位置方差
         deg2rad(1.0)  % 航向角方差
         1.0 % 速度方差
         ])^(2);

%量测噪声
R = diag([
           3 % GPS X量测噪声
           3 % GPS Y量测噪声
            ])^(2);

%  输入噪声参数(速度传感器(编码器) 和 角速度传感器(陀螺仪))
global  INPUT_NOISE
INPUT_NOISE  = diag([1.0, deg2rad(30.0)]) ^(2);

%GPS 噪声参数
global GPS_NOISE;
GPS_NOISE = diag([0.7, 0.7]) ^(2);

dt = 0.1;     %积分间隔
N = 400;

%% 初始值
x_est = zeros(4, 1);     %卡尔曼估计值
x_gt = zeros(4, 1);       %真实值
P = eye(4);                 %过程噪声矩阵
x_dr = zeros(4,1);       %航迹推算值

for i = 1:N
    
    % 生成理想传感器数据 u = [v yawrate]' 速度和偏航率
    u = calc_input(); 
    
    %生成真实轨迹 x_gt， 带有噪声的观测值ud，带有噪声的生成轨迹
    [x_gt, z, x_dr, ud] = observation(x_gt, x_dr, u, dt); 
    
    [x_est , P] = ekf_estimation(x_est, P, Q, R, z, ud, dt); %EKF滤波
    
    % 记录历史数据
    hx_gt(i,:) = x_gt;
    hz(i,:) = z;
    hx_dr(i,:) = x_dr;
    hx_est(i,:) = x_est;
    hPtrace(i,:) = trace(P);
end

%% plot结果
figure;
hold on;
plot(hx_gt(:,1), hx_gt(:,2));
plot(hx_dr(:,1), hx_dr(:,2));
plot(hz(:,1), hz(:,2),'.');
plot(hx_est(:,1), hx_est(:,2));
legend("真实值(GT)", "航迹推算(DR)", "观测值(GPS)", "EKF滤波");

figure;
hold on;
plot(1:N, hx_gt(:,3));
plot(1:N, hx_dr(:,3));
plot(1:N, hx_est(:,3));
title("航向角");
legend("真实值(GT)", "航迹推算(DR)" , "EKF滤波");

figure;
hold on;
plot(1:N, hx_gt(:,4));
plot(1:N, hx_dr(:,4),'g');
plot(1:N, hx_est(:,4),'b.');
title("速度");
legend("真实值(GT)", "航迹推算(DR)" , "EKF滤波");


figure;
plot(1:N, hPtrace);
title("P迹(系统稳定度)");


%计算误差
pos_diff = hx_est(:,1:2) - hx_gt(:,1:2);
err = sum( sum(abs(pos_diff).^2,2).^(1/2));
fprintf("滤波位置与真实值误差:%f m\n", err);


%% 子函数

% EKF滤波过程
function  [x_est , P]  = ekf_estimation(x_est, P, Q, R, z, u, dt)

% 预测状态
x_est = motion_model(x_est, u, dt);

%误差矩阵预测
JF = jacob_f(x_est, u, dt);   % 求状态过程,对状态方程求雅克比矩阵
P = JF*P*JF' + Q;              

% 更新
JH = jacob_h();
z_pred = observation_model(x_est);


% 计算卡尔曼增益
S = JH * P * JH' + R;
K = P * JH' * S^(-1);

% 通过测量更新估计
x_est = x_est + K*( z - z_pred);

% 更新误差方差
P = (eye(length(x_est)) - K*JH) * P;
end


%观测模型
function z =  observation_model(x)
H = [1, 0, 0, 0;  0, 1, 0, 0];
z = H * x;
end

%观测过程Jacob
function J= jacob_h()
% 观测模型的Jacobian
J= [1 0 0 0; 0 1 0 0];

end

%求状态过程Jacob
function J = jacob_f(x, u, dt)
yaw = x(3);
v = x(4);
J = [1      0    -dt*v*sin(yaw),     dt*cos(yaw); 
       0      1     dt*v*cos(yaw)     dt*sin(yaw); 
       0      0         1                           0; 
       0      0         0                           1];

end

%生成 真实值，DR值和观测值
function  [x_gt, z, x_dr, u_dr] = observation(x_gt, x_dr, u, dt)

% 运动模型  x = F*x + B*u;
x_gt =  motion_model(x_gt, u, dt);

% 添加测量噪声
global GPS_NOISE;
% 观测模型z = H * x;
z = observation_model(x_gt) + GPS_NOISE* randn(2,1);

% 添加传感器噪声
global INPUT_NOISE;
u_dr = u + INPUT_NOISE * randn(2,1);

% 生成真实DR轨迹
x_dr = motion_model(x_dr, u_dr, dt);

end

% 运动模型
function x =  motion_model(x, u, dt)
F =  [
    1, 0, 0, 0;
    0, 1, 0, 0;
    0, 0, 1, 0;
    0, 0, 0, 0];

yaw = x(3);
B = [dt*cos(yaw) 0; dt*sin(yaw) 0; 0 dt; 1 0];
x = F*x + B*u;
end

%生成传感器输入
function u =  calc_input()

v = 1.0;  % [m/s]
yawrate = 0.1; % [rad/s]

u = [v yawrate]';
end

运行结果：

位置估计：

 航向角估计：

速度估计：

系统稳定度（过程噪声矩阵）：

---

3.一些参数比较

3.1 GPS噪声

gps定位误差0.1米

gps定位误差1米

gps定位误差2米

结论：GPS精度越高，EKF的估计值越准确，相反精度越低，越会误导EKF

3.2  Q阵

3.2.1 修改位置方差

Q = diag([  %预测状态协方差
          0.1  % x轴上的位置方差
          0.1  % y轴上位置方差
         deg2rad(1.0)  % 航向角方差
         1.0 % 速度方差
         ])^(2);

 %过程噪声
Q = diag([  %预测状态协方差
          3  % x轴上的位置方差
          3  % y轴上位置方差
         deg2rad(1.0)  % 航向角方差
         1.0 % 速度方差
         ])^(2);

 结论：修改Q阵的前两项，值越大EKF的预测轨迹越贴近GPS,预测轨迹抖动越大。

其它测试待续，直观看不出来有啥影响，Q阵R阵调节的特别大，也只是抖动，也不太影响准确度，以后有机会再试试。。。。未完待续