一、算法介绍
卡尔曼滤波是一个神奇的滤波算法，应用非常广泛，它是一种结合先验经验、测量更新的状态估计算法。
1、状态估计
首先，对于一个我们关心的物理量，我们假设它符合下面的规律
其中，为该物理量本周期的实际值，为该物理量上一个周期的实际值，当然这个物理量可能不符合这个规律，我们只是做了一个假设。不同的物理量符合的规律不同，是我们的经验，我们根据这个规律可以预测我们关心的物理量。比如，我们关心的物理量是车速，如果车辆接近匀速运动时，则的取值为1，也就是这个周期与上个周期的速度相同。
下面我们再来看一下这个物理量的测量公式
其中，是这个物理量的测量值，是测量噪声。我们对一个物理进行预测，测量是一个必不可少的手段，虽然测量的不一定准，但是在很大程度上体现了物理量的实际值。这个公式体现的就是实际值与测量值的关系。还是以车速为例，是通过车速传感器得到的测量值。
实际中，物理量一般不会像我们上面的公式那样简单，一般我们用下面的公式来表示
其中，uk 代表了处理噪声，这个噪声是处理模型与实际情况的差异，比如车速，他会受到人为加速、减速、路面不平等外界因素的影响。
卡尔曼滤波的基本思想是综合利用上一次的状态和测量值来对物理量的状态进行预测估计。我们用来x^k表示Xk的估计值，则有下面的公式
在这个公式中，综合利用了上一个周期的估计值和这个周期的测量值来对进行估计。其中，叫gk为卡尔曼增益，这个公式与一阶滤波很相似，只不过卡尔曼增益是会变的，每个周期都会更新，一阶滤波的系数则是固定值。考虑极端的情况来分析增益的作用，当gk = 0,增益为0，这时，这表示我们这个周期的估计值与上个周期是相同的，不信任当前的测量值；当gk = 1时，增益为1，这时x^k = x^k-1，这表示我们这个周期的估计值与测量值是相同的，不信任上个周期的估计值，在实际应用时，介于0~1之间，它代表了对测量值的信任程度。
2、卡尔曼增益
上面我们通过卡尔曼增益来估计物理量的值，那卡尔曼增益又是如何取值的呢？我们通过下面两个公式来计算并在每个周期进行迭代更新。
3、完整卡尔曼滤波算法
有了上面的推导，我们在下面列出来完成卡尔曼滤波的公式，卡尔曼滤波分为预测过程和更新过程两个过程，在公式中，我们又引入了缩放系数，和协方差。
上面的公式适合一维变量的卡尔曼滤波，将变量扩展到多维，用向量和矩阵替换上面的变量，就可以实现多维变量的卡尔曼滤波，下面的公式适用于多维变量。
二：matlab代码（亲测有效）
% ==================== 系统描述 ====================
% GPS精度为4m
% 加速度计的精度为 1cm/s^2，且有 3cm/s^2 的偏移
% 真实的加速度为 2cm/s^2
% 数据采集频率皆为 10Hz
% ================== 模拟产生数据 ==================
clear all
rng(0); % 设置随机数种子
dt = 0.1; % 0.1s采集一次数据
T = 0:dt:100; % 0~100s
N = length(T); % 采样点数
a = 2e-2; % 真实加速度
s = 1/2 * a * T.^2; % 真实位移（实际上不可知）
aa = a + 1e-2*randn(size(s)) + 3e-2; % 加速度计测量数据，有噪声，有偏移1
R = 4; % GPS 测量精度
z = s + sqrt(R)*randn(size(s)); % GPS 测量数据，开根号得到
% ================= 变量定义与初始化 =================
A = [1 dt; 0 1]; % 状态转移矩阵
H = [1 0]; % 转换矩阵
B = [1/2*dt^2; dt]; % 输入控制矩阵
Q = [0.001 0; 0 0.01]; % 过程噪声协方差，估计一个
%%
P = eye(2); % 初始值为 1（可为非零任意数）
%%
x = zeros(2, N); % 存储滤波后的数据，分别为位移、速度信息
k = 1; % 采样点计数
% ================= 卡尔曼滤波过程 ==================
for t = dt:dt:100
k = k + 1
x(:,k) = A * x(:,k-1) + B*aa(k); % 卡尔曼公式1
P = A * P * A' + Q; % 卡尔曼公式2
K = P*H' * inv(H*P*H' + R); % 卡尔曼公式3
x(:,k) = x(:,k) + K * (z(:,k)-H*x(:,k)); % 卡尔曼公式4
P = (eye(2)-K*H) * P; % 卡尔曼公式5
end
% ==================== 结果绘图 =====================
figure(1);
plot(T, z(1,:),'b.');hold on; % GPS测量数据
plot(T, x(1,:),'r.'); % 滤波后数据
plot(T, s ,'k-'); % 绘制真实值
legend('滤波前','滤波后','理想值'); % 标注
xlabel('时间: s');
ylabel('距离: m');hold off;
axis([0 100 -20 120])
text(2, 110, ['滤波前方差: ' num2str(var(z - s))]) % 滤波前方差
text(2, 100, ['滤波后方差: ' num2str(var(x(1,:) - s))]) % 滤波后方差
figure(2);
plot(T(1:end-1), (x(1,2:end)-x(1,1:end-1))/dt, 'b.');hold on% 将滤波后位移差分结果
plot(T, x(2,:), 'r.'); % 滤波估计速度
plot(T, a*T, 'k-'); % 真实速度
legend('滤波位移差分','卡尔曼估计值','真实值')
xlabel('时间: s');
ylabel('速度: m');hold off;
axis([0 100 -8 10])
text(2, 9, ['位移差分方差: ' ...
num2str(var((x(1,2:end)-x(1,1:end-1))/dt - a*T(1:end-1)))]) % 滤波前方差
text(2, 7.6, ['速度滤波方差: ' ...
num2str(var(x(2,:) - a*T))]) % 滤波后方差
引用：
https://blog.csdn.net/weixin_41869763/article/details/105792692
https://blog.csdn.net/bhniunan/article/details/104607668?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522163970238816780366533676%2522%252C%2522scm%2522%253A%252220140713.130102334…%2522%257D&request_id=163970238816780366533676&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2alltop_click~default-1-104607668.pc_search_insert_es_download_v2&utm_term=%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2&spm=1018.2226.3001.4187