卡尔曼滤波示例

卡尔曼滤波5个公式，看起来简单，但想弄明白并不容易。从直接理解上，个人感觉比较有帮助的是这篇博客：http://blog.csdn.net/u013453604/article/details/50301477 ；但卡尔曼滤波本质上是一种linear dynamical systems，想理解透彻最好还是搞清楚这个东西，可参考PRML第13章。

现在先把最简单的示例传上来，后续根据linear dynamical systems的内容还需要进一步研究其参数配置优化方法。

如下是根据网上流传（百度百科）的温度预测范例程序修改后的结果，主要改进是：原程序中温度一直保持24度不变，这种情况下预测量是1维（温度）；本程序修改温度为时变值，这种情况下要求预测量成为2维（温度和变化间隔），状态转移矩阵A也成为矩阵，而不是简单的1。

计算结果如下：

主程序：

clear all; close all; clc;

%% 
% web: http://blog.csdn.net/foreseerwang
% QQ: 50834

%% 初始化参数
n_iter = 300;       % 计算连续n_iter个时刻
n = 2;              % [温度，变化间隔]
m = 1;              % 温度

x = zeros(n, n_iter);           % 状态量，每个时刻包含两个值：温度，变化间隔
P = zeros(n, n, n_iter);        % 文档及变化间隔的方差
z = zeros(m, n_iter);           % 观测量

xrate=0.01;                     % 温度变化率
xreal = 24+(0:n_iter-1)*xrate;  % 温度变化间隔为1
z = xreal + randn(m,n_iter);    % 观测到的温度，误差方差为1

kf_para.A=[1,xrate;0,1];        % 状态转移矩阵，根据上一时刻预测当前时刻值
kf_para.H = zeros(m,n);         % 观测矩阵，把m维观测量转换成n维状态量
kf_para.H(1)=1;
kf_para.Q = [4e-4, 0; 0, 1e-4]; % 预测误差的方差
kf_para.R = 0.25;               % 观察误差的方差

x(:,1)=[23.5; 0.9];             % 初始化，第一个时刻的温度及变化间隔
P(:,:,1)=diag(rand(n,1));

%% 卡尔曼滤波迭代
for ii = 2:n_iter,

    [x(:,ii), P(:,:,ii)]=kf_iter(kf_para, x(:,ii-1), P(:,:,ii-1), z(ii));

end

%% 结果打印
figure;
plot(x(1,:),'r*'); hold; plot(z,'bo'); plot(xreal, 'k.');
legend('卡尔曼滤波后的温度', '观测温度', '真实温度');
title('温度对比');
ylabel('温度');

figure;
plot(x(2,:));
title('滤波后的温度变化间隔（真实值为1）');

fprintf('观测误差均值为：%5.3f；均方差为：%5.3f\n', mean(abs(z-xreal)), std(z-xreal));
fprintf('滤波误差均值为：%5.3f；均方差为：%5.3f\n', mean(abs(x(1,:)-xreal)), std(x(1,:)-xreal));

卡尔曼滤波函数：

function [ x, P ] = kf_iter( kf_para, x_k_1, P_k_1, z )
% kf_para：卡尔曼滤波器的参数，A、H、Q、R
% x_k_1，P_k_1：上一观测时刻的最优预测值及方差
% z：当前时刻的观测值
% x, P：当前时刻的最优预测值及方差

n=size(x_k_1,1);

% 根据前值预测当前值
xbar = kf_para.A*x_k_1;
Pbar=kf_para.A*P_k_1*kf_para.A' + kf_para.Q;

%计算卡尔曼增益
K = Pbar*kf_para.H'*pinv(kf_para.H*Pbar*kf_para.H'+kf_para.R); 

% 根据卡尔曼增益和当前观测值修订预测值，获得当前最优预测值
x=xbar+K*(z-kf_para.H*xbar);
P=(diag(ones(n,1))-K*kf_para.H)*Pbar;

end