卡尔曼滤波的C及MATLAB实现——一维Kalman滤波为例

最近用到Kalman滤波算法，只需要简单应用，不求详细的数学证明，总结如下：

1、 谁是卡尔曼

卡尔曼并不遥远，他是一个当代人，卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。

2、 卡尔曼滤波器的历史

卡尔曼滤波器最早在Kalman的《A New Approach to Linear Filtering and Prediction Problems》(线性滤波与预测问题的新方法)论文中提出；卡尔曼滤波器是一个"optimal recursive data processing algorithm（最优化自回归数据处理算法）" 对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如人脸识别，图像分割，图像边缘检测等等。

3、 卡尔曼滤波器小结

需要了解几个数学知识：

1、每一个随机变量X都是具有一个平均值（这个值在变量的概率密度函数分布图的最中心位置，代表该数值是最可能发生的）和方差（这个数值代表变量的不确定性程度）。平均值μ就是数学期望μ=E(X)。方差D(X)=σ^2=E{[x-E(x)]^2}是衡量或一组数据时离散程度的度量。方差的平方根即标准差或者均方差，即σ。

2、两正态分布的乘积仍然是正态分布，且新正态分布的均值和方差如下：

即正态分布的线性叠加定理。

3、两个事件的发生都是概率性的，不能完全相信其中的任何一个！如果我们具有两个事件，从直觉或者是理性思维上讲，是不是认定两个事件都发生，就找到了那个最理想的估计值？好了，抽象一下，得到：两个事件同时发生的可能性越大，我们越相信它！要想考察它们同时发生的可能性，就是将两个事件单独发生的概率相乘。Kalman滤波算法的本质就是利用两个正态分布的融合仍是正态分布这一特性进行迭代。

4、对Q、R值的理解。 Q是预测值的方差，R是测量值的方差。两者Q/(Q+R)决定Kalman增益，kalman增益越大（说明测量值越可信）实时性好，受随机值影响大；反之（预测值越可信），滞后越大，越平滑

4、 Kalman滤波算法的C代码

float process_variance=0.005,measure_variance=0.1;//过程方差 测量方差

float posteriori_estimation=0,priori_estimation=0;//后验估计值 先验估计值

float posteriori_estimatio_variance=0;//后验估计的方差

float priori_estimatio_variance=0;//先验估计的方差

float kalman_gain=0;//

void KalmanFilter_Init_4()

{

posteriori_estimation=0;

posteriori_estimatio_variance=0;//后验估计的方差

priori_estimation=0;//先验估计值

priori_estimatio_variance=0;//先验估计的方差

kalman_gain=0;

posteriori_estimation=0 ;

posteriori_estimatio_variance=0;

}

float KalmanFilter_4(float Measure)

{

//时间更新（预测）

priori_estimation = posteriori_estimation;// 用上一时刻的最优估计值来作为对当前时刻的预测

priori_estimatio_variance = posteriori_estimatio_variance + process_variance;// 预测的方差为上一时刻最优估计值的方差与过程方差之和

//测量更新（校正）

kalman_gain = priori_estimatio_variance/(priori_estimatio_variance + measure_variance); //计算卡尔曼增益

posteriori_estimation = priori_estimation + kalman_gain*(Measure-priori_estimation);//结合当前时刻的测量值，对上一时刻的预测进行校正，得到校正后的最优估计。该估计具有最小均方差

priori_estimatio_variance = (1-kalman_gain) *priori_estimatio_variance;//计算最终估计值的方差

posteriori_estimatio_variance = priori_estimatio_variance;//

return posteriori_estimation;

}

5、 Kalman滤波算法的M代码

clear all;

close all;

% intial parameters

length = 45; %计算连续length个时刻

sz = [length, 1];

x = 0; %初值，

Q = 0.1; %过程方差

R = 0.5; %测量方差

z=[42 -91 -152 15 15 -133 8 -76 1933 2179 1800 1952 2168 1982 1868 1990 2043 2043 2423 1864 1990 1974 1963 1940 1948 2088 2013 1963 2221 2008 2028 1762 2301 1936 2096 1883 1925 1906 1975 1963 2070 2058 1902 2065 1990];

%对数组进行初始化

x_filter=zeros(sz); %后验估计。即在k时刻，结合当前测量值与k-1时刻先验估计，得到的最终估计值

P_filter=zeros(sz); %后验估计的方差

x_Prediction=zeros(sz); %先验估计。即在k-1时刻，对k时刻做出的估计

P_Prediction=zeros(sz); %先验估计的方差

K=zeros(sz); %卡尔曼增益，反应了测量结果与过程模型的可信程度

% intial guesses

x_filter(1) = 0; %初始估计值为0度

P_filter(1) =0; %误差方差为0

for k = 2:length

%时间更新（预测）

x_Prediction(k) = x_filter(k-1); %用上一时刻的最优估计值来作为对当前时刻的预测

P_Prediction(k) = P_filter(k-1)+Q; %预测的方差为上一时刻最优估计值的方差与过程方差之和

%测量更新（校正）

K(k) = P_Prediction(k)/( P_Prediction(k)+R ); %计算卡尔曼增益

x_filter(k) = x_Prediction(k)+K(k)*(z(k)-x_Prediction(k)); %结合当前时刻的测量值，对上一时刻的预测进行校正，得到校正后的最优估计。该估计具有最小均方差

P_filter(k) = (1-K(k))*P_Prediction(k); %计算最终估计值的方差

end

FontSize=10;

LineWidth=3;

figure();

plot(z,'r+'); %画出的测量值

hold on;

plot(x_filter,'b-','LineWidth',LineWidth) %画出最优估计值

%画出真实值

legend('测量值', '后验估计');

xl=xlabel('序号');

yl=ylabel('测量值');

set(xl,'fontsize',FontSize);

set(yl,'fontsize',FontSize);

hold off;

set(gca,'FontSize',FontSize);

输出结果如图

可见Kalman滤波算法有较好的响应速度和滤波效果。