卡尔曼滤波

卡尔曼滤波

卡尔曼滤波的核心

卡尔曼滤波的核心是预测+测量反馈。

 

以一个匀速直线运动的过程为例:

 

目标系统(线性系统)的状态转移方程:

                                                                                         

Xtt时刻,小车的位置;

Utt时刻对系统的控制量,在本例中可以理解为小车行驶的速度。

AB为系统参数;

转移过程中存在的噪声为Q

Tips:产生一个随机分布的指定均值和方差的矩阵:将randn产生的结果乘以标准差,然后加上期望均值即可。例如,产生均值为0.6,方差为0.1的一个5*5的随机数方式如下:x = .6 + sqrt(0.1) * randn(5);

t=0时刻小车的位置服从正态分布:



t=1时刻小车的位置利用转移方程得到,并添加了过程噪声:



目标系统的观测方程:

状态的测量必须是状态的线性函数叠加高斯噪声

                                                                                                       

Ztt时刻的测量值;

C为测量系统的参数,可以理解为线性系数;

NoiseR)以R为方差的高斯噪声。

t=1时刻测距传感器的的测量值(蓝色部分,同样服从正态分布):


加权合并

加权滤波是滤波中一种比较常见的方式,核心在于加权系数的选择,在通常的加权滤波算法中,该权值由人来制定,在卡尔曼滤波中通过计算得到。

图中的绿色部分为合并后的小车位置,服从正态分布,可以作为下一轮迭代的初始位置。

卡尔曼滤波是一种递归的估计,即只要获知上一时刻状态的估计值以及当前状态的观测值就可以计算出当前状态的估计值,因此不需要记录观测或者估计的历史信息。


计算过程:

通过状态转移方程预测:


预测值的误差相关矩阵:

噪声是以方差的形式参与计算。

预测的方差为上一时刻的最优估计方差与高斯噪声方差之和。


计算卡尔曼增益:


当系统为单输入输出的时候


计算误差量:


更新状态变量:

更新误差相关矩阵:

 



Matlab仿真:



% 初始化参数
n_iter = 100;       %计算连续n_iter个时刻
sz = [n_iter, 1]; 
Q = 4e-4;           % 过程转移噪声
R = 0.25;           % 测量噪声,反应测量器件的精度
speed = 1;          % 小车行驶速度,系统的控制量
positon = 0;        % 小车的位置;


real_position = zeros(sz);
for k = 1:n_iter
    real_position(k) = k-1;
end
measure_position = real_position + sqrt(R)*randn(sz); %距离的测量值为距离的真实值加上方差为0.25的高斯噪声


position_predict = zeros(sz);  %通过状态转移方程计算出的小车位置
variance_predict = zeros(sz);  %计算过程的方差


position_kalman = zeros(sz);  %位置经过kalman后的最优估计值
variance_kalman = zeros(sz);  %卡尔曼最优估计的方差
position_kalman(1) = 100;       %首轮迭代的初始值为0
variance_kalman(1) = 1;       %初始最优估计的方差为1, 这里设置的大一点,好体现效果。


kalman_gain = zeros(sz);  %kalman增益


for k = 2:n_iter
    position_predict(k) = position_kalman(k-1) + speed + sqrt(Q)*randn(1);    %用上一时刻的最优估计值结合状态转移方程对本时刻小车的位置进行预测
    variance_predict(k) = variance_kalman(k-1) + Q;        %预测的方差为上一时刻的最优估计方差与高斯噪声方差之和
    kalman_gain(k) = variance_predict(k)/(variance_predict(k) + R);  % 计算卡尔曼增益
    %结合测量值和卡尔曼增益对预测结果进行矫正
    position_kalman(k) = position_predict(k) + kalman_gain(k) * (measure_position(k) - position_predict(k));
    %更新kalman最优估计的方差
    variance_kalman(k) = (1 - kalman_gain(k))*variance_predict(k);
end
%绘图相关。。。。。
FontSize=14;
LineWidth=3;
figure();
plot(measure_position,'k+'); %画出温度计的测量值
hold on;
plot(position_kalman,'b-','LineWidth',LineWidth) %画出最优估计值
hold on;
plot(real_position,'g-','LineWidth',LineWidth); %画出真实值
legend('位置测量值', 'Kalman估计值', '位置真实值');
xl=xlabel('时间(分钟)');
yl=ylabel('位置');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
hold off;
set(gca,'FontSize',FontSize);
figure();
valid_iter = [2:n_iter]; % variance_pre not valid at step 1
plot(valid_iter,variance_kalman([valid_iter]),'LineWidth',LineWidth); %画出最优估计值的方差
legend('Kalman估计的误差估计');
xl=xlabel('时间(分钟)');
yl=ylabel('℃^2');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
set(gca,'FontSize',FontSize);


相关文章
相关标签/搜索