详解卡尔曼滤波算法

详解卡尔曼滤波算法

理解：https://www.zhihu.com/question/23971601/answer/375355599

实例：http://bilgin.esme.org/BitsAndBytes/KalmanFilterForDummies

 

根据实例中的数据，做如下计算：

time_ms = [1 2 3 4 5 6 7 8 9 10];
% 测量值
measure_value = [0.39 0.50 0.48 0.29 0.25 0.32 0.34 0.48 0.41 0.45];
calc_value = [];

% 初始值
X_k_1 = 0;
P_k_1 = 1;

% 外界原因对温度的影响
Q = 0.1;
% 传感器的测量方差
R = 0.1;

% X_k_1 k-1 时刻状态
% X_k   k 时刻状态
% X_k_  预测k 时刻状态
% P_k_  预测k 时刻不确定度（方差）
% P_k   k 时刻的不确定度（方差）
for i = time_ms
    X_k_ = X_k_1;
    P_k_ = P_k_1 + Q;
    K_k = P_k_/(P_k_ + R);
    X_k = X_k_ + K_k * (measure_value(i) - X_k_);
    P_k = (1 - K_k) * P_k_;
    
    P_k_1 = P_k;
    X_k_1 = X_k;
    
    calc_value = [calc_value X_k];
end

figure;

plot(time_ms, measure_value, 'r')
hold on;
plot(time_ms, calc_value, 'b')
axis([1 10 0 0.6]);

最终结果的波动总比测量值小，并跟随测量值变化。

通过调节Q的值，可以调节最终结果：

Q = 0，最终结果变动量更小；

Q = 1，最终结果基本等于测量值，由于原值基本为0.4，Q = 1时，说明外界因素对温度影响大，最终结果基本

是测量值，还是比较科学的。

 

逐行理解上述两个链接的内容，基本能搞懂卡尔曼滤波是个什么样的过程。