卡尔曼滤波由目标位置信息，估算它的位置和速度信息

一个简单说明

 

这是我在做车辆速度估算中所用到的技术做个总结，并附上仿真代码。

主要用途为，知道了小车的每一帧二维位置信息 (posX, posY)，经过kalman滤波，输出更新后的小车位置信息 (posX, posY) 以及它分解速度信息 (Vx, Vy)。

一、预测

在首帧数据来之前，我们是不知道小车的状态的，所以随便猜一个它的初始状态（或者滤波后，滤波结果就是状态），X = [1,  1,  0,  0]'，(表示初始位置为(1, 1)，x,y方向的速度为(0, 0))，X是一个需要更新的量

状态转移矩阵为:A =  [1, 0, T, 0; 0, 1, 0, T; 0, 0, 1, 0; 0, 0, 0, 1]，为一个4*4的矩阵，其中T为帧与帧的时间间隔，A是一个不需要更新的量

状态控制矩阵为: B = [T^2/2, T^2/2, T, T]'，控制量: u = 4e-3（这个量可根据实际情况进行修改), B是一个不需要更新的量

状态协方差：S = state_uncertain * eye(4)，是一个4*4的矩阵，表示4个状态间的不确定度，是一个需要更新的量, state_uncertain = 5, (这个随机设置就行，反正后面会更新矩阵S)

过程激励噪声协方差：Q = [T^2/4, 0, T^3/2, 0; 0, T^2/4, 0, T^3/2; T^3/2, 0, T^2, 0; 0, T^3/2, 0, T^2]，（为什么是这个形式，我也不知道，反正好用），是一个不需要更新的量

1.状态预测

X = A*X + B*u,   矩阵维度为:  (4*1) = (4*4)*(4*1) + (4*1) * (1*1)

2.状态协方差预测

S = A*S*A'+Q,  矩阵维度为: (4*4) = (4*4) * (4*4) * (4*4)' + (4*4)

二、更新

有了上一步的预测状态和预测协方差，再有当前的小车的位置输入input (是一个2*1的矩阵，分别为位置信息posX, posY)来对位置和速度进行预测

这里还需要状态转移矩阵：H = [1, 0,  0, 0; 0, 1, 0, 0]'， 是一个不需要更新的量

测量噪声矩阵：R =meas_uncertain* [1, 0; 0, 1]，表示测量噪声协方差，是一个不需要更新的量，meas_uncertain=1（控制测量误差的，可以修改看滤波效果）

1.计算kalman增益

K = S*H'*inv(H*S*H'+R), 矩阵维度为：(4*2) = (4*4) * (2*4)' *inv((2*4) * (4*4) * (2*4)' + (2*2))， 是一个需要更新的量

2.更新状态

X = X + K*(input - H*X) ,矩阵维度为：(4*1) = (4*1) + (4*2) * ((2*1) - (2*4) * (4*1)) ,这里卡尔曼增益就相当于是一个修正权重，所乘的那一坨就是当前输入值与预测值的误差，然后来修正输入，也就是滤波过程

3.更新状态协方差

S = (eye(4) - K*H)*S，矩阵维度为：(4*4) = (4*4 - (4*2) * (2*4)) * (4*4)

整个滤波过程就就是一个迭代的过程，由上一帧的滤波结果X（4*1矩阵包含[posX, posY, Vx, Vy]）和输入状态input (2*1矩阵 包含[posX, posY])，先预测状态X和状态协方差，再由输入对状态和协方差进行更新，得到更新后的状态，经过多帧更新以后就能得到较为准确的 Vx和Vy，因此只需要有位置信息就能得到 X，Y方向的速度信息以及平滑后的位置信息。

Matlab仿真实验

话不多说，直接上代码干

clc;
clear all;
close all;

%begin
dt=0.1;
%状态转移矩阵
A = [1 0 dt 0;
    0 1 0 dt;
    0 0 1 0 ;
    0 0 0 1 ;
    ];
%状态控制矩阵
B = [(dt^2)/2 (dt^2)/2 dt dt]';
u = 4e-3;
%状态到测量的转换矩阵
H = [1 0 0 0;
    0 1 0 0];

State_Uncertainty = 5;
%状态协方差
S = State_Uncertainty * eye(size(A,1));
%系统过程协方差 状态转换与实际过程的误差 也可认为是噪声
Q = [(dt^2)/4       0       (dt^3)/2       0;
    0       (dt^2)/4        0       (dt^3)/2;
    (dt^3/2)        0         (dt^2)        0;
    0           (dt^3)/2        0        (dt^2);
    ];

% Covariance Matrix R
Meas_Unertainty = 1;
%测量噪声协方差矩阵
R = Meas_Unertainty * eye(size(H,1));


Kalman_Output = [];
%初始化状态
x = [1; 1; 0; 0]; % Initial Values

flag = 2;

true_vx = [];
true_vy = [];
if flag == 1%常速模型 垂直车身运动
    %造一批数据
    temp_x = -100:100; %设置为目标速度 vx = 10m/s 帧率10，每帧运动1m
    temp_y = 50;
    a = rand(1,size(temp_x, 2))*5;
    temp_y = temp_y + a;
    data = [temp_x;temp_y]';
end

if flag == 2 %常速模型，倾斜运动
    id = 1:200; %取200个采样点
    temp_vx = 4;
    temp_vy = 3;
    temp_x = -80;
    temp_y = 20;
    for i = 1:size(id, 2)
        true_vx = [true_vx, temp_vx];
        true_vy = [true_vy, temp_vy];
        temp_x = [temp_x, temp_x(end)+temp_vx*dt];
        temp_y = [temp_y, temp_y(end)+temp_vy*dt];
    end
    a = rand(1,size(temp_x, 2))*3;
    temp_x = temp_x + a;
    a = rand(1,size(temp_y, 2))*4;
    temp_y = temp_y + a;
    
    data = [temp_x; temp_y]';
end

if flag == 3 %匀加速模型
    id = 1:100; %取200个采样点
    vx_init = 1;
    vy_init = 1;
    ax = 0.2;
    ay = 0.1;
    temp_x = -50;
    temp_y = 20;
    for i = 1:size(id, 2)
        vx_init = vx_init + ax;
        true_vx = [true_vx, vx_init];
        vy_init = vy_init + ay;
        true_vy = [true_vy, vy_init];
        temp_x = [temp_x, temp_x(end) + vx_init*dt];
        temp_y = [temp_y, temp_y(end) + vy_init*dt];
    end
    a = rand(1,size(temp_x, 2))*4;
    temp_x = temp_x + a;
    a = rand(1,size(temp_y, 2))*3;
    temp_y = temp_y + a;
    data = [temp_x; temp_y]';
end


N = size(data, 1);
% figure
predict_vx = [];
predict_vy = [];
for i = 1:N
    input = data(i, :)';
    scatter(input(1), input(2), 'g');
    axis([-110 110 0 80])
    hold on
    
    % Estimate the next state
    x = A*x + B*u; %(4*4)*(4*1) + (4*1)*(1*1)
    
    % Estimate the error covariance
    S = A*S*A' + Q;
    
    % Kalman Gain Calculations
    K = S*H'*inv(H*S*H'+R);
    
    x = x + K*(input - H*x); %输出为[x, y, vx, vy]
    
    predict_vx = [predict_vx, x(3)];
    predict_vy = [predict_vy, x(4)];
    % Update the error covariance
    S = (eye(size(S,1)) - K*H)*S;
    scatter(x(1), x(2), 'r');
    axis([-110 110 0 80])
    hold on
    
end

flag = 2;% 0绘制原始值, 1绘制残差, 2绘制残差比例

if flag == 0
    figure
    subplot(1,2,1)
    plot(true_vx, 'g')
    hold on
    plot(predict_vx, 'r')
    title('vx预测')
    
    subplot(1,2,2)
    plot(true_vy, 'g')
    hold on
    plot(predict_vy, 'r')
    title('vy预测')
end
if flag == 1
    figure
    subplot(1,2,1)
    plot(true_vx - predict_vx(1,1:end-1), 'g')
    title('vx残差')
    subplot(1,2,2)
    plot(true_vy - predict_vy(1,1:end-1), 'g')
    title('vy残差')
end
if flag == 2
    figure
    subplot(1,2,1)
    plot((true_vx - predict_vx(1,1:end-1))./true_vx, 'g')
    title('vx残差')
    subplot(1,2,2)
    plot((true_vy - predict_vy(1,1:end-1))./true_vy, 'g')
    title('vy残差')
end

可以得到如下的仿真结果

左边为滤波位置(posX, posY)的仿真结果，绿色点为输入位置信息，红色为kalman滤波的结果，表现出来的效果为，小车的位置不会有剧烈点抖动，位置信息较为平滑，

右侧为预测速度的误差比例，因为造数据的时候是知道小车的速度信息，但是滤波的整个过程，我们是不会告诉程序小车的速度，完全是由位置信息来计算出小车分分解速度。总体而言在进行滤波10次左右便会得到一个误差非常小的分解速度信息。

还可以通过修改程序来方针匀速直线运动，匀加速运动等，我测试过该模型都能更好的预测目标的轨迹以及速度信息。

声明：这里所记录的都是基于我自己的理解，理解的都比较肤浅，有些描述可能也是错误的，如有错误或者更好的建议，欢迎指出。