卡尔曼滤波算法入门+实战_卡尔曼滤波的q和r怎么计算-优快云博客

本文链接：https://blog.youkuaiyun.com/pang_fighting/article/details/139412840

写在前面，方差，度量单个变量的离散程度

$\delta _{x}^{2} = \frac{1}{n-1}\sum_{i=1}^{n}(x_{i} - \bar{x})^{2}$

协方差，度量两个变量变化趋势的相似程度

$cov(x,y)=\frac{1}{n-1}\sum_{i=1}^{n}(x_{i}-\bar{x})(y_{i}-\bar{y})$

特别地，单个变量的协方差就是自己的方差， $cov(x,x) = \delta _{x}^{2}$ 。

———————————————————————————————————————————卡尔曼滤波的核心思想是利用上一状态的系统估计以及当前状态的测量结果来求当前状态的最优估计值，所以卡尔曼滤波最后得到的结果是最优估计！

下面介绍卡尔曼滤波中的几个公式：

$X_{k}=AX_{k-1} + BU_{k}+w_{k}$ (1-1)

公式（1-1）被称为状态方程， $X_{k}$ 为当前时刻的估计状态， $X_{k-1}$ 为前一时刻的状态， $U_{k}$ 为当前时刻系统的输入， $w_{k}$ 为服从 $N(\mu ,Q)$ 的正态分布的噪声， $A$ 和 $B$ 为系数矩阵。

$Y_{k}=CX_{k}+v_{k}$ (1-2)

公式（1-2）被称为观测方程， $Y_{k}$ 为当前时刻状态的测量值， $C$ 为观测矩阵， $v_{k}$ 为服从 $N(\mu ,R)$ 的测量噪声。

在实际应用中 $U_{k}$ 这个输入一般为0，于是就有

$X_{k}^- = AX_{k-1} +w_{k}$ (1-3)

这里 $X_{k}^-$ 其实和之前公式（1-1）里面的 $X_{k}$ 一样，只是在后面为了说明方便换了一下，叫做先验状态估计，对（1-3）算协方差得到

$P_{k}^- = AP_{k-1}A^T+Q$ (1-4)

推导就不推了。接着是卡尔曼增益公式

$K_{k} = P_{k}^- C^T(CP_{k}^-C^T+R)^{-1}$ (1-5)

有了卡尔曼增益，我们就可以计算出想要的最终的当前状态最优估计 $X_{k}$ ：

$X_{k}=X_{k}^-+K_{k}(Y_{k}-CX_{k}^-)$ (1-6)

计算完当前状态的最优估计之后我们就来到最后一步更新 $P_{k}$ ：

$P_{k} = P_{k}^--K_{k}CP_{k}^-$ (1-7)

梳理一下全部流程，首先利用公式(1-3)及公式(1-4)算出预测值，然后利用公式(1-5)及公式(1-6)算出最优估计值（也就是最终滤波结果），最后利用公式(1-7)更新预测重新回到(1-3)，不断循环。

下面具体实操一下，针对AD采集到的数据进行卡尔曼滤波。

首先分析一下，对于AD采集数据来说，只有一个变量电压值，也就是说，这个问题下卡尔曼滤波为一维，其中 $A$ ， $C$ 都为1，于是公式(1-3)变成 $X_{k}^-=X_{k-1}$ ，这里将 $w_{k}$ 给忽略了，公式(1-4)变成 $P_{k}^-=P_{k-1}+Q$ ，公式(1-5)变成 $K_{k}=P_{k}^-(P_{k}^-+R)^{-1}$ ，公式(1-6)变成 $X_{k}=X_{k}^-+K_{k}(Y_{k}-X_{k}^-)$ ，公式(1-7)变成 $P_{k}=P_{K}^--K_{k}P_{k}^-$ ， $Y_{k}$ 就是AD采集到的电压值的大小，下面开始编程：

clear;
close all;
format long
%数据采集及处理
LP01=readmatrix('./LP_01.csv');%读取LP01的数据
N=size(LP01,1);%计算M的行数  1代表行维度 返回行的数量
x01=LP01(1:N,1); %取出LP01的第一列放入x01中
y01=LP01(1:N,2); %取出LP01的第二列放入x02中

%一维卡尔曼滤波
K=zeros(N,1);    %卡尔曼增益
X=zeros(N,1);    %滤波后的值
P=zeros(N,1);    %误差协方差矩阵
Q=1e-6;     %过程噪声协方差
R=1e-3;     %观测噪声协方差
X(1)=0; %给定初值，一般给0
P(1)=1;      %给定误差协方差初值，一般给1
for i=2:N %滤波之后第一个数可以忽略
    K(i)=P(i-1)/(P(i-1)+R);
    X(i)=X(i-1)+K(i)*(y01(i)-X(i-1));
    P(i)=P(i-1)-K(i)*P(i-1)+Q;
end
plot(x01, y01, 'b', x01, X, 'r');  %绘制滤波后的图像
legend('before', 'after');