卡尔曼滤波
卡尔曼滤波知识点
o什么是卡尔曼滤波？
你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。
对现在时刻的估计（可能是同时估计好几个变量）是取决于前一时刻估计误差和现在时刻的某个观测值
在连续变化的系统中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点（除了前一个状态量外，不需要保留其它历史数据），并且速度很快，很适合应用于实时问题和嵌入式系统。
在Google上找到的大多数关于实现卡尔曼滤波的数学公式看起来有点晦涩难懂，这个状况有点糟糕。
实际上，如果以正确的方式看待它，卡尔曼滤波是非常简单和容易理解的，下面我将用漂亮的图片和色彩清晰的阐述它，你只需要懂一些基本的概率和矩阵的知识就可以了。
o公式
卡尔曼滤波有两组基本的方程，状态运动方程和观测方程，这是需要预先给定的。
状态运动方程
oX(k)=A(k) X(k-1)+B U(k)+W(k)
X(k)是k时刻的系统状态，
U(k)是k时刻对系统的控制量。
A称 为状态转移矩阵。
可以看出状态运动方程反映了k-1时刻目标的状态与k时刻目标状态之间的关系，所以我们称之为状态运动方程。
再加上系统的测量值：
oZ(k)=H (k)X(k)+V(k)
Z(k)是k时刻的观测值，
H是观测矩阵，
表示人们对于k时刻状态进行观测时，获取观测量的机制。
W(k)和V(k)分别表示状态方程和观测方程的噪声。
在应用卡尔曼滤波时，首先必须要给出运动方程和观测方程，也就是说，要对目标运动及观测过程进行建模，并且模型是线性的，称之为 “Model-based” 。
这是卡尔曼滤波最大的一个缺陷。
假设W和V为高斯白噪声(White Gaussian Noise)，他们的covariance 分别是Q，R（这里我们假设他们不随系统状态变化而变化）。
o编程公式
首先我们要利用状态方程预测下一状态的系统。
假设当前时刻为k，根据系统的模型，可以基于系统的上一状态而预测出现在状态：
X(k|k-1)=A X(k-1|k-1)+B U(k) ………… (1)
X(k|k-1)是利用上一状态预测的结果
X(k-1|k-1)是上一状态最优的结果
U(k)为现在状态的控制量，
o如果没有控制量，它可以为0
我们研究的变量在这一时刻与上一时刻的具体关系。那个Fai矩阵就是系数对应关系。可以看到这里就把卡尔曼滤波的使用范围限定在线性系统中了，基本的卡尔曼滤波器是不能用于非线性系统的。
到现在为止，我们的系统结果已经更新了，可是，对应于X(k|k-1)的covariance，即上面预测值相对于真实状态的误差方差阵更新公式：
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
oP(k|k-1)是X(k|k-1)对应的covariance
oP(k-1|k-1)是X(k-1|k-1)对应的covariance
covariance
协方差
oA’表示A的转置矩阵
oQ是系统过程的covariance
o其实就是计算估计值和实际值的方差大小，以评估现在的估计的误差是不是合适。
当第k时刻的观测值Z(k)到达以后，我们希望利用它去修正第k个时刻的状态预测值。
怎么去修正呢？
当然是要用预测值中没有的信息去修正预测值了。
o因此Z(k)-H X(k|k-1)就是新的信息。
o结合预测值和测量值，我们可以得到现在状态(k)的最优化估算值X(k|k)：
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain)：
到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。
求要估计的值
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：
卡尔曼滤波根据当前观测变量的变化趋势来估计一时刻变量的值，这个增益矩阵就是指我们定的这个变化的“度”的大小。
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)
其中I 为1的矩阵，对于单模型单测量，I=1。
当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。
修正估计值与实际值的方差
这样，算法就可以自回归的运算下去。
卡尔曼滤波器的原理基本描述了，式子1，2，3，4和5就是他的5 个基本公式。根据这5个公式，可以很容易用计算机编程实现。
o假设一个接近匀速运动的小球，初始位置为24，既然是“接近匀速”，必有误差，误差为0.04，状态转移矩阵是常数1。如果有一个仪器对位移进行测量，获得的是位移值，则观测矩阵也是1，测量一定有误差，其误差为0.25.从下面的图可以看出估计值即滤波与其真实值很接近，但是量测值与真实值相去甚远。
作业
oQ4. A temperature sensor is used to measure the temperature in a refrigerator over time. The sensor has a noise of = 0.5摄氏度 and the goal is to keep the temperature around 4摄氏度 through some control mechanism.
温度传感器用于随时间测量冰箱中的温度。传感器的噪音 = 0.5摄氏度，目标是通过某种控制机制将温度保持在 4摄氏度 左右。
传感器的噪音 = 0.5摄氏度
通过某种控制机制将温度保持在 4摄氏度
oThe sensor output over a given period is provided in the assignment folder
(download the TemperatureSensor.csv file where the left column is the sample number and the right one is measured temperatures).
指定时间段内的传感器输出在分配文件夹中提供
oa) Design a Kalman filter to smoothen the signal. Plot the following parameters over the measurement period:
a) 设计一个卡尔曼滤波器来平滑信号。绘制测量周期内的以下参数：
1. Actual sensor output;
2. Kalman filter output;
3. Kalman gain.
卡尔曼的增益
ob) Compare the noise from the sensor and that of the Kalman filter output
b) 比较来自传感器的噪声和卡尔曼滤波器输出的噪声
oc) We know that at some point in the measurement the temperature increased but fell back to its desired value after a while.
oc) 我们知道在测量的某个时刻，温度升高，但一段时间后又回落到其期望值。
Can you identify this event from the sensor data?
你能从传感器数据中识别出这个事件吗？
How about from the Kalman filtered data?
卡尔曼滤波后的数据怎么样？
What was the increase in temperature?
温度升高了多少？
卡尔曼滤波在温度测量中的应用
o题目
房间温度在25摄氏度左右，测量误差为正负0.5摄氏度，方差0.25，R=0.25。Q=0.01,A=1,T=1,H=1。
假定快时刻的温度值、测量值为23.9摄氏度，房间真实温度为24摄氏度，温度计在该时刻测量值为24.5摄氏度，偏差为0.4摄氏度。
利用k-1时刻温度值测量第k时刻的温度，
o卡尔曼滤波公式
X(k)=AX(k-1)+TW(k-1)
Z(k)=HX(k)+V(k)
o计算过程
其预计偏差为:
P(k|k-1)=P(k-1)+Q=0.02
卡尔曼增益
k=P(k|k-1) / (P(k|k-1) + R)=0.0741
X(k)=23.9+0.0741(24.1-23.9)=23.915摄氏度。
k时刻的偏差为P(k)=(1-KH)P(k|k-1)=0.0186。
最后由X(k)和P(k)得出Z(k+1)。
o代码
% 程序说明：Kalman滤波用于温度测量的实例
参数定义
ofunction main
oN=120;
oCON=25;%房间温度在25摄氏度左右
房间温度在25摄氏度左右，
测量误差为正负0.5摄氏度，
oXexpect=CONones(1,N);
假定快时刻的温度值、
偏差为0.4摄氏度。
oX=zeros(1,N);
oXkf=zeros(1,N);
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain)：
到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。
求要估计的值
oZ=zeros(1,N);
Z(k)=HX(k)+V(k);
H=1;
oP=zeros(1,N);
oX(1)=25.1;
房间真实温度为24摄氏度，
oP(1)=0.01;
Q=0.01,
oZ(1)=24.9;
测量值为23.9摄氏度，
温度计在该时刻测量值为24.5摄氏度，
oXkf(1)=Z(1);
oQ=0.01;%W(k)的方差
方差0.25，
oR=0.25;%V(k)的方差
R=0.25。
oW=sqrt(Q)randn(1,N);
oV=sqrt®randn(1,N);
oF=1;
F就是A
oG=1;
oH=1;
oI=eye(1);
卡尔曼滤波公式定义
o%%%%%%%%%%%%%%%%%%%%%%%for k=2:N X(k)=FX(k-1)+GW(k-1); Z(k)=HX(k)+V(k); X_pre=FXkf(k-1); P_pre=FP(k-1)F’+Q; Kg=P_preinv(HP_preH’+R); e=Z(k)-HX_pre; Xkf(k)=X_pre+Kge; P(k)=(I-KgH)P_pre;end
X(k)=FX(k-1)+GW(k-1);
X(k|k-1)=A X(k-1|k-1)+B U(k) ………… (1)
oX(k|k-1)是利用上一状态预测的结果
oX(k-1|k-1)是上一状态最优的结果
oU(k)为现在状态的控制量，
如果没有控制量，它可以为0
我们研究的变量在这一时刻与上一时刻的具体关系。那个Fai矩阵就是系数对应关系。可以看到这里就把卡尔曼滤波的使用范围限定在线性系统中了，基本的卡尔曼滤波器是不能用于非线性系统的。
X(k)=A(k) X(k-1)+B U(k)+W(k)
oX(k)是k时刻的系统状态，
oU(k)是k时刻对系统的控制量。
oA称 为状态转移矩阵。
o可以看出状态运动方程反映了k-1时刻目标的状态与k时刻目标状态之间的关系，所以我们称之为状态运动方程。
F=1;
F就是A
oA称 为状态转移矩阵。
G=1;
G是B
X(1)=25.1;
房间真实温度为24摄氏度，
o Z(k)=HX(k)+V(k);
H=1;
再加上系统的测量值：
Z(k)=H (k)X(k)+V(k)
oZ(k)是k时刻的观测值，
oH是观测矩阵，
o表示人们对于k时刻状态进行观测时，获取观测量的机制。
o X_pre=FXkf(k-1);
Xkf(1)=Z(1);
o P_pre=FP(k-1)F’+Q;
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
P(k|k-1)是X(k|k-1)对应的covariance
P(k-1|k-1)是X(k-1|k-1)对应的covariance
ocovariance
协方差
A’表示A的转置矩阵
Q是系统过程的covariance
其实就是计算估计值和实际值的方差大小，以评估现在的估计的误差是不是合适。
增益矩阵
o Kg=P_preinv(HP_preH’+R);
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
但是为了要令卡尔曼滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下X(k|k)的covariance：
卡尔曼滤波根据当前观测变量的变化趋势来估计一时刻变量的值，这个增益矩阵就是指我们定的这个变化的“度”的大小。
e=Z(k)-HX_pre;
Xkf(k)=X_pre+Kge;
oX(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
其中Kg为卡尔曼增益(Kalman Gain)：
到现在为止，我们已经得到了k状态下最优的估算值X(k|k)。
求要估计的值
P(k)=(I-KgH)*P_pre;
oP(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)
其中I 为1的矩阵，对于单模型单测量，I=1。
当系统进入k+1状态时，P(k|k)就是式子(2)的P(k-1|k-1)。
修正估计值与实际值的方差
o%%%%%%%%%%%%%%%%Err_Messure=zeros(1,N);Err_Kalman=zeros(1,N);for k=1:N Err_Messure(k)=abs(Z(k)-X(k)); Err_Kalman(k)=abs(Xkf(k)-X(k));end
o作图
t=1:N;
figure(‘Name’,‘Kalman Filter Simulation’,‘NumberTitle’,‘off’);plot(t,Xexpect,’-b’,t,X,’-r’,t,Z,’-k’,t,Xkf,’-g’);legend(‘expected’,‘real’,‘measure’,‘kalman extimate’); xlabel(‘sample time’);ylabel(‘temperature’);title(‘Kalman Filter Simulation’);
figure(‘Name’,‘Error Analysis’,‘NumberTitle’,‘off’);plot(t,Err_Messure,’-b’,t,Err_Kalman,’-k’);legend(‘messure error’,‘kalman error’); xlabel(‘sample time’);%%%%%%%%%%%%%%%%%%%%%%%%%
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
X(k|k-1)=A X(k-1|k-1)+B U(k) ………… (1)
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (4)
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)
P(k|k-1)=A P(k-1|k-1) A’+Q ……… (2)
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) ……… (3)
X(k|k-1)=A X(k-1|k-1)+B U(k) ………… (1)
编程公式
2332
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
