python的filterpy包中卡尔曼滤波的使用代码

一、场景设置与效果图

假设一架无人机携带了一个气压计和GPS，定位精度都为1m，数据采样频率都为5Hz，100s时间螺旋上升40m，螺旋半径为20m。在无人机飞行过程中，1m的精度是不能很好满足要求的，现对采样得到的数据使用卡尔曼滤波，使得达到更高的精度。

最终效果如下：

二、卡尔曼滤波

卡尔曼滤波器假设一个系统当前时刻的状态是由上一时刻的状态和输入转化过来的，当然，这个过程中包含着噪声；我们可以使用传感器测量这个状态，但是测量过程也有噪声。那么，该系统可以如下定义：

现在的情况是已经有了测量值z(k)，想要估计系统的真实状态x(k)。

三、MATLAB代码

为了方便第一次实现，首先是一维（仅高度）的情况

% GPS精度为1m、气压计精度为0.5m，加速度计的精度为 1cm/s^2
% 无人机按照螺旋线飞行，半径为 20m，螺距为40m，100s完成一圈飞行
% 数据采集频率为 10Hz

clear all

D = 1;                          % 维度，1

dt = 0.1;                       % 0.1s采集一次数据
t0 = 0:dt:100;                  % 0~100s
N = length(t0);                 % 采样点数

A = eye(2);                     % 状态转移矩阵
x = zeros(D, N);                % 存储滤波后的数据
z = ones(D, N);                 % 存储滤波前的数据
x(:, 1) = ones(D,1);            % 初始值设为 1（可为任意数）
P = eye(D);                     % 初始值为 1（可为非零任意数），取 D阶单位矩阵
      
Q = 0.01*eye(D);                % 过程噪声协方差，估计一个
R = 1;                          % 测量噪声协方差，精度为多少取多少
 
k = 1;                          % 采样点计数

true1D = t0*0.4;                % 一维仅高度，气压计数据

for t = dt:dt:100
    k = k + 1;                  
    x(:,k) = A * x(:,k-1) ;      % 卡尔曼公式1
    P = A * P * A' + Q;         % 卡尔曼公式2
    H = eye(D);
    K = P*H' * inv(H*P*H' + R); % 卡尔曼公式3                            
    z(:,k) = true1D(k) + randn; % 一维仅高度（z方向）               
    x(:,k) = x(:,k) + K * (z(:,k)-H*x(:,k));    % 卡尔曼公式4
    P = (eye(D)-K*H) * P;                       % 卡尔曼公式5
end

                                        
plot(t0, z,'b.');                           % 绘制滤波前数据
axis('equal');hold on;grid on;              % 坐标等距、继续绘图、添加网格
plot(t0, x,'r.');                           % 绘制滤波后数据
plot(t0, true1D ,'k-');                     % 绘制真实值
legend('滤波前','滤波后','理想值');           % 标注
xlabel('时间: s');                         
ylabel('高度: m');hold off;

下面是1~3维都可以使用的代码，其实添加的内容不多，跟一维很类似，只不过多了几个条件判断

% GPS精度为1m、气压计精度为0.5m，加速度计的精度为 1cm/s^2
% 无人机按照螺旋线飞行，半径为 20m，螺距为40m，100s完成一圈飞行
% 数据采集频率为 10Hz

clear all

D = 3;                          % 维度，可取 1,2,3

dt = 0.1;                       % 0.1s采集一次数据
t0 = 0:dt:100;                  % 0~100s
N = length(t0);                 % 采样点数

A = eye(D);                     % 状态转移矩阵，和上一时刻状态没有换算，故取 D阶单位矩阵
x = zeros(D, N);                % 存储滤波后的数据
z = ones(D, N);                 % 存储滤波前的数据
x(:, 1) = ones(D,1);            % 初始值设为 1（可为任意数）
P = eye(D);                     % 初始值为 1（可为非零任意数），取 D阶单位矩阵
      
r = 20;                         % 绕圈半径，20m
w = 2*pi / 100;                 % 计算出角速度，100s绕一圈

Q = 1e-2*eye(D);                % 过程噪声协方差，估计一个
R = [1 0 0;
     0 1 0;
     0 0 0.5];                  % 测量噪声协方差，精度为多少取多少
 
k = 1;                          % 采样点计数

if D==1                         % 一维仅高度，气压计数据
    true1D = t0*0.4;
elseif D==2                     % 二维 x,y 方向，GPS数据
    true2D = [r*cos(w*t0); r*sin(w*t0)];
elseif D==3                     % 三维 x,y,z方向，GPS和气压计
    true3D = [r * cos(w*t0); r * sin(w*t0); t0 * 0.4];
end

for t = dt:dt:100
    k = k + 1;                  
    x(:,k) = A * x(:,k-1);      % 卡尔曼公式1
    P = A * P * A' + Q;         % 卡尔曼公式2
    H = eye(D);
    K = P*H' * inv(H*P*H' + R); % 卡尔曼公式3
    if D==1                                     % 一维仅高度（z方向）
        z(:,k) = true1D(k) + randn;                 
    elseif D==2                                 % 二维 x,y 方向
        z(:,k) = [true2D(1,k) + randn; true2D(2,k) + randn];
    elseif D==3                                 % 三维 x,y,z 方向
        z(:,k) = [true3D(1,k) + randn; true3D(2,k) + randn; true3D(3,k) + randn];
    end
    x(:,k) = x(:,k) + K * (z(:,k)-H*x(:,k));    % 卡尔曼公式4
    P = (eye(D)-K*H) * P;                       % 卡尔曼公式5
end

if D==1                                         %% 一维情况
    plot(t0, z,'b.');                           % 绘制滤波前数据
    axis('equal');hold on;grid on;              % 坐标等距、继续绘图、添加网格
    plot(t0, x,'r.');                           % 绘制滤波后数据
    plot(t0, true1D ,'k-');                     % 绘制真实值
    legend('滤波前','滤波后','理想值');           % 标注
    xlabel('时间: s');                         
    ylabel('高度: m');hold off;  
elseif D==2                                     %% 二维情况
    plot(z(1,:),z(2,:),'b.');                   % 绘制滤波前数据
    axis('equal');grid on;hold on;              % 坐标等距、继续绘图、添加网格
    plot(x(1,:),x(2,:),'r.');                   % 绘制滤波后数据
    plot(true2D(1,:), true2D(2,:), 'k.');       % 绘制真实值
    legend('滤波前','滤波后','理想值'); 
    xlabel('x方向: m');
    ylabel('y方向: m');hold off;
elseif D==3                                     %% 三维情况
    plot3(z(1,:),z(2,:),z(3,:),'b.');           % 绘制滤波前数据
    axis('equal');grid on;hold on               % 坐标等距、继续绘图、添加网格
    plot3(x(1,:),x(2,:),x(3,:),'r.');           % 绘制滤波后数据
    plot3(true3D(1,:), true3D(2,:), true3D(3,:));% 绘制滤波后数据
    legend('滤波前','滤波后','理想值');           % 绘制真实值
    xlabel('x方向: m');
    ylabel('y方向: m');
    zlabel('高度: m');hold off;
end

代码中，可以改变 D 的值查看不同维度情况。其实，在2维或3维情况下，各个维度完全是相互独立的，不支持矩阵运算时（比如C语言直接写），分别算几次就好。