卡尔曼滤波详解：一维卡尔曼滤波实例解析(五个公式以及各个参数的意义)

一、引言

本文以rssi(接收信号强度)滤波为背景，结合卡尔曼的五个公式，设计 rssi 一维卡尔曼滤波器，用MATLAB语言实现一维卡尔曼滤波器，并附上代码和滤波结果图；

本文工分为以下几个部分：

1、引言

2、模型的系统方程和状态方程

3、卡尔曼滤波过程及五个基本公式

4、公式中每个参数详细注释

5、结合rssi滤波实例设计滤波器

6、MATLAB实现滤波器

二、模型的系统方程和状态方程

系统的状态方程：

$\LARGE {\color{DarkBlue} }x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}$

状态方程是根据上一时刻的状态和控制变量来推测当前时刻的状态， $\large w_{k-1}$ 是服从高斯分布的噪声，是预测过程的噪声，它对应了 $\large x_k$ 中每个分量的噪声，是期望为 0，协方差为 Q 的高斯白噪声 $\large w_{k-1} \sim N_{(0, Q))}$ ,Q即下文的过程激励噪声Q.

系统的观测方程：

$\LARGE z_k = Hx_k + v_k$

观测方式是当前时刻的量测信息， $\large v_k$ 是观测的噪声，服从高斯分布， $\large v_k \sim N_{(0,R)}$ ,R即下文的测量噪声R。

卡尔曼滤波算法有两个基本假设:

( 1) 信息过程的足够精确的模型，是由白噪声所激发的线性( 也可以是时变的) 动态系统;

( 2) 每次的测量信号都包含着附加的白噪声分量。当满足以上假设时，可以应用卡尔曼滤波算法。

三、卡尔曼滤波过程及五个基本公式

卡尔曼滤波时间更新（预测）
1. 向前推算状态变量

$\LARGE \hat{x}_{k}^{-} = A\hat{x}_{k-1} + Bu_{k-1}$

2. 向前推算误差协方差

$\LARGE P_{k}^{-} = AP_{k-1} A^{T} + Q$

卡尔曼滤波测量更新（校正）
3. 计算卡尔曼增益

$\LARGE K_k = \frac{P_{k}^{-}{H^T}}{HP_{k}^{-}H^T + R}$

4. 由观测变量 $\large z_k$ 更新估计

$\LARGE \hat{x}_k = \hat{x}_{k}^{-} + K_k(z_k - H\hat{x}_{k}^{-})$

5. 更新测量误差

$\LARGE P_k = (I - K_kH)P_{k}^{-}$

四、公式中每个参数详细注释

$\LARGE \hat{x}_{k-1}$ 和 $\LARGE \hat{x}_{k}$ ：分别表示 $\LARGE k-1$ 时刻和 $\LARGE k$ 时刻的后验状态估计值，是滤波的结果之一，即更新后的结果，也叫最优估计（估计的状态，根据理论，我们不可能知道每时刻状态的确切结果所以叫估计）。
$\LARGE \hat{x}_{k}^{-}$ ： $\LARGE k$ 时刻的先验状态估计值，是滤波的中间计算结果，即根据上一时刻（ $\LARGE k-1$ 时刻）的最优估计预测的 $\LARGE k$ 时刻的结果，是预测方程的结果。
$\LARGE P_{k-1}$ 和 $\LARGE P_k$ ：分别表示 k － 1 时刻和 k 时刻的后验估计协方差（即 $\LARGE \hat{x}_{k-1}$ 和 $\LARGE \hat{x}_k$ 的协方差，表示状态的不确定度），是滤波的结果之一。
$\LARGE P_{k}^{-}$ ： k 时刻的先验估计协方差（ $\LARGE \hat{x}_{k}^{-}$ 的协方差），是滤波的中间计算结果。
$\LARGE H$ ：是状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系，卡尔曼滤波里为线性关系，它负责将 m 维的测量值转换到 n 维，使之符合状态变量的数学形式，是滤波的前提条件之一。
$\LARGE z_k$ ：测量值（观测值），是滤波的输入。
$\LARGE K_k$ ：滤波增益矩阵，是滤波的中间计算结果，卡尔曼增益，或卡尔曼系数。
$\LARGE A$ ：状态转移矩阵，实际上是对目标状态转换的一种猜想模型。例如在机动目标跟踪中，状态转移矩阵常常用来对目标的运动建模，其模型可能为匀速直线运动或者匀加速运动。当状态转移矩阵不符合目标的状态转换模型时，滤波会很快发散。
$\LARGE Q$ ：过程激励噪声协方差（系统过程的协方差）。该参数被用来表示状态转换矩阵与实际过程之间的误差。因为我们无法直接观测到过程信号，所以 Q 的取值是很难确定的。是卡尔曼滤波器用于估计离散时间过程的状态变量，也叫预测模型本身带来的噪声。状态转移协方差矩阵。
$\LARGE R$ ：测量噪声协方差。滤波器实际实现时，测量噪声协方差 R一般可以观测得到，是滤波器的已知条件。
$\LARGE B$ ：是将输入转换为状态的矩阵。
$\LARGE (z_k - H\hat{x}_{k}^{-})$ ：实际观测和预测观测的残差，和卡尔曼增益一起修正先验（预测），得到后验。

五、结合rssi滤波实例设计滤波器

1、建立模型系统方程和量测方程

由于分析对象是无线信号的一维rssi状态，所以具体空间过程不关心，只需要从发射端发射到接收端接收是没有其他控制状态的，但是在传输过程中是存在噪声的，根据公式

$\LARGE {\color{DarkBlue} }x_k = Ax_{k-1} + Bu_{k-1} + w_{k-1}$

可得：A 为[1]，B为[0]， $\large w_{k-1}$ 为高斯白噪声可不关心

2、建立量测方程

由于接收设备可直接输出rssi值，根据公式

$\LARGE z_k = Hx_k + v_k$

可得：H为[1]， $\large v_k$ 为量测噪声可不关心

3、分析Q和R

假如 $\large w$ 已经分析出一系列的数据，则 $\large Q = cov(w)$ ;

假如 $\large v$ 已经分析出一些列的数据，则 $\large R = cov(v)$ ;

4、初始值确定

给滤波过程的初始状态初始化。

5、最后一步，就是对照公式根据理解，套公式，写程序。

六、MATLAB实现滤波器

1、滤波器设计

function z = kalmanFilter(x)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  function z = kalmanFilter(x)
%
%>
%> @brief 一维卡尔曼滤波
%>
%> @param[out]  z             滤波后的结果
%> @param[in]   x             需要滤波的数据
%>
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % 卡尔曼相关变量定义
    persistent xk xk_1;             % 状态量
    persistent zk;                  % 观测量
    persistent A;                   % 状态转移矩阵
%     persistent B;                   % 控制输入模型
    persistent H;                   % 观测矩阵
    persistent Pk Pk_1;             % 误差协方差矩阵
    persistent Q;                   % 状态噪声协方差矩阵
    persistent R;                   % 观测噪声协方差矩阵
    
    % 卡尔曼相关参数初始化
    if isempty(xk)
        A = 1;
        H = 1;
        Pk = 1;
        Pk_1 = 1;                   % 初始误差协方差为1
        Q = 0.01;                   % 反应两个时刻rssi方差
        R = 0.05;                   % 反应测量rssi的测量精度
        xk = 0;
        xk_1 = 0;
        zk = 0;
    end
    
    I = 1;
    if xk_1 == 0
        xk_1 = x;
        xk = x;
    else
        zk = H*x;                   % 观测量方程
        % 预测
        X = A*xk_1;                 % 状态预测
        P = A*Pk_1*A' + Q;          % 误差协方差预测
        % 更新(校正)
        K = P*H'*inv(H*P*H'+R);     % 卡尔曼增益更新
        xk = X + K*(zk - H*X);      % 更新校正
        xk_1 = xk;                  % 保存校正后的值，下一次滤波使用
        Pk = (I - K*H)*P;           % 更新误差协方差
        Pk_1 = Pk;                  % 保存校正后的误差协方差，下一次滤波使用
    end
    
    % 滤波结果返回
    z = xk;
end

2、仿真运行

%% 1. 导入数据
[fname, pname] = uigetfile('*', 'Sample Dialog Box');
fileID = fopen(strcat(pname, fname));
data = cell2mat(textscan(fileID,'%f%f','delimiter', ',','headerlines',0));
rssi = data(:, 1);

%% 2. 卡尔曼滤波
rssi_opt = zeros(size(rssi,1), 1);
for k = 1:size(rssi,1)
    rssi_opt(k) = kalmanFilter(rssi(k));
end

%% 3. 滤波检验
figure(1);
plot(rssi, 'Color', 'r', 'Marker', 'o'); hold on;
plot(rssi_opt, 'Color', 'b', 'LineStyle', '-', 'Marker', '+'); hold off;
legend('rssi原始波形', 'rssi经过滤波后的波形');
title('rssi滤波对比');

3、运行结果

卡尔曼滤波详解：一维卡尔曼滤波实例解析(五个公式以及各个参数的意义)