一阶卡尔曼学习记录

1.引言

    控制系统的测量信号总是包含测量噪声和其他扰动信号。传统的卡尔曼滤波器是一种线性滤波器，能处理测量值和受控量是线性关系的滤波过程，快速过滤信号中的白噪声，提高控制系统的稳定性和控制精度。卡尔曼滤波是时域估计方法，能对时变、非平稳信号、多维信号进行处理，不需要频域变换，采用递推算法，运算量小，存贮量小，实现简单。

2.卡尔曼滤波器原理

卡尔曼有五个基本公式，前两个是预测公式，后三个是更新公式，卡尔曼就是通过计算预测值和测量值的方差之比来估计预测值和测量值占真值的权重，即更相信哪个值。下面用通俗的说法说明一阶卡尔曼公式。

用k-1时刻的值来预测当前k时刻的状态值X(K|K-1)和误差协方差P (K|K-1)， (K|K-1)表示用K-1时刻的值来算K时刻，表示是预测的。

(K-1|K-1)表示K-1时刻的最优值。(K|K)表示K时刻的最优值，是最终需要得到的值。

预测公式：

X(K|K-1)  = X(K-1|K-1)        假设每一时刻的值是不变化的。

P(K|K-1)  = P(K-1|K-1)+Q      Q是过程噪声，代表测得预测值的准确与否，要小

更新公式：

Kg =P(K|K-1) /(P(K|K-1) +R)      R是测量噪声，代表测量噪声，即采集数据的波动

        Kg是卡尔曼增益。R越小，Kg越大，表示越相信测量值，Kg代表相信测量值的程度

X(K|K) = (1 – Kg) · X(K|K-1) + Kg · Z(K)

                                 相信预测值1-Kg，相信测量值Kg。算出此刻的最优值

   P(K|K) = (1 – Kg) · P(K|K-1)        更新此刻的协方差。相信原来值的 1-Kg 。

3.卡尔曼的matlab仿真

代码和注释如下：

N = 200;

x(1)=25;

for k = 2:N;

    x(k) = 25+5*randn;    %25附近的一个信号，均方差是5

end

klm.Q =0.01;                过程噪声       

                                             代表测得预测值的准确与否，也就是模型一定要准确。要是你预测和原来相差太多，就失去

                          了预测的意义。这个值越小，滤波后的曲线越平稳 . 一般来说，这个值是 ADC 自身的跳动。

klm.R =25;                  测量噪声   

                                             代表你测得不准，也就是说这个值应该和信号的白噪声方差差不多。这个值可以自适应 . 这个

                          值越大，滤波后的曲线越平稳。一般来说，这个值是信号的跳动。初始时， R= 信号方差。如

                          果方差越大， R 就要减少来适应。要更贴合上升或者下降曲线

klm.x_last = x(1);

klm.p_last = 0;

klm.x_now(1)= x(1);

 

for t = 2:N;

klm.x_pre = klm.x_last;

 klm.p_pre =klm.p_last + klm.Q;  认为这个值不会变化，但实际会有这么大的变化，

 klm.kg      = klm.p_pre / (klm.p_pre + klm.R);

            R 是测量噪声，如果 R 很小， kg 就无限接近于 1 ，就认为测得的就是真实曲线。起不到

            滤波的效果。代表预测的值的方差占总方差中的比例，也就是不相信预测值的比

            例，也就是相信测量值的比例

klm.x_now(t) = (1 - klm.kg) * klm.x_pre +klm.kg * x(t);

            %真实值就等于相信x(t) kg，相信预测值1-kg

 klm.p_now=  (1 - klm.kg) * klm.p_pre;

            %预测的p里面就有1-kg是可以被信任的。

 klm.p_last =klm.p_now;

 klm.x_last = klm.x_now(t);

end

t=1:N;

plot(t,x,'r',t,klm.x_now);holdon;

 

结果如下：

滤波后的值大约是25，很接近真值，说明卡尔曼滤波是起作用的。

4.QR参数对卡尔曼滤波的影响

    klm.Q = 0.01;         

Q是过程噪声，代表测得预测值的准确与否，也就是模型一定要准确。要是你预测和原来相差太多，就失去了预测的意义。这个值越小，滤波后的曲线越平稳。越大，越接近测量曲线。一般来说，这个值是ADC自身的跳动。Q = 0时代表你预测100%准确，曲线就是一条水平线。

    klm.R = 25;          

R测量噪声，代表你测得不准，也就是说这个值应该和信号的白噪声方差差不多。这个值可以自适应，这个值越大，滤波后的曲线越平稳，越小越接近测量曲线。一般来说，这个值是信号的方差。初始时，R=信号方差。如果方差越大，R就要减少来适应，要更贴合上升或者下降曲线。

Q = 0:0.05:1

R = 25

曲线如图 所示：

Q = 0时，为中间的水平线，预测完全正确，滤波器输出的值一直都是初始值25。

     逐渐增大的过程中，滤波后的曲线越来越接近测量曲线。

       

      这个值取小一些比较好，根据实际情况而定，可以是测量工具本身的误差。比如 ADC 采集一个噪声很小的固定电压有一定的波动， Q 等于这个波动的方差 。或者调试时，逐个试一下，从小到大试。

Q = 0.01

R = 0:5:50

曲线如图 所示：

 R= 0时，无滤波作用，输出就是测量曲线，表示测量得完全不准，滤波器一点都不相信它测量的值。

    R逐渐减小的过程中，滤波后的曲线越来越接近测量曲线。这个值并不是越大越好，越大曲线越平滑，对响应不敏感；R越小曲线越接近测量值，VOD调节越频繁，但响应快。

       

     这个值取大一些比较好，根据实际情况而定，通常是先计算 200 个测量点的方差，取这个方差作为初始R 。或者调试时，逐个试一下，从大到小调试。

5.卡尔曼在单片机中的实现

    建立一个kalman结构体；

typedef struct{

   float kg;

   float Q;

   float R;

   float x_pre;

   float p_pre;

   float p_last;

   float p_now;

   float x_now;

   float x_last;

   float x_pid;

}KALMAN;

 

KALMAN klm;

 

unsigned long Kal_Man(unsigned long res)//采集值

{

klm.x_pre =klm.x_last;

   klm.p_pre = klm.p_last + klm.Q;

   klm.kg     = klm.p_pre / (klm.p_pre+ klm.R);

   klm.x_now = (1 - klm.kg) * klm.x_pre + klm.kg * res;

   klm.p_now = (1 - klm.kg) * klm.p_pre;

   klm.p_last = klm.p_now;

   klm.x_last = klm.x_now;

 

   return klm.x_now;

}

 

    提前对klm.x_last 赋值一个测量值，klm.p_last 赋值一个比较小的值，但不能为0.