卡尔曼滤波简介及例子

Posted 2021-01-31 winslam

    卡尔曼滤波在大学课程《现代控制理论》当中有涉及详细讲解。卡尔曼滤波使用条件有：1、线性系统； 2、系统中噪声(不确定性)服从高斯分布。下文中的方差、误差、偏差、协方差都指不确定性的意思。误差 = 偏差；方差 = 偏差 2。在无人驾驶导航定位当中，需要多传感器对汽车位姿进行检测 ；在多传感器融合方面，使用卡尔曼滤波理论较多。

1、5个公式

 2、汽车例子

 

 1 clear all;
 2 close all;
 3 clc;
 4 Z = (1:100); %观测值
 5 noise = randn(1,100);%1行100列高斯白噪声
 6 Z = Z + noise; 
 7 
 8 X = [0;0];%状态值
 9 P = [1 0; 0 1];%状态协方差矩阵
10 F = [1 1; 0 1];%状态转移矩阵
11 Q = [0.0001, 0; 0 0.0001];%状态转移协方差矩阵
12 H = [1 0];%观测矩阵
13 R = 1;%观测噪声方差
14 
15 figure;
16 %hold on;
17 speed = [];
18 distance = [];
19 for i =1:100
20    %% 预测
21    X_ = F*X;
22    P_ = F*P*F‘ + Q;
23    %% 更新
24    K = P_*H‘/(H*P_*H‘ + R);
25    X = X_ + K*(Z(i) - H*X_);
26    P = (eye(2) - K*H)*P_;
27 
28    speed(i) = X(2);
29    distance(i) = X(1);
30    %%plot(X(1), X(2));
31 end
32 plot(distance, speed);

 

3、温度估计例子

 

 

 

 参考：https://blog.csdn.net/u010720661/article/details/63253509