卡尔曼（kalman）详解

kalman详解

---

---

贝叶斯准则(Bayes rule)

全概率定理

两个随机变量 X 和 Y 的联合分布(joint distribution)如下:
p (x , y) = p (X = x , Y = y)（X=x，Y=y同时发生的概率）
如果 X和 Y 相互独立 (independent) , 有
p(x,y) =p(x)p(y)

---

假定已经知道 Y 的值是 y, 想知道基于以上事实条件 X 为 x 的概率。这样的概率表示为
p (x I y) = p (X = x I Y = y)
称为条件概率 (conditional probability)
如果p(y)>0 有p (x I y)=p(x , y) / p(y)
如果x y独立 则有p (x I y)=p( x )p( y ) / p(y)=p(x)
p(x) = LP(x I y)p(y) (离散情况)
p(x) = Jp(x I y)p(y)dy (连续情况)

贝叶斯

贝叶斯在通常的概率推测中起着主导作用。如果a是一个希望由b推测出来的数值，则概率p（a）被称为先验概率分布。b为数据，一般是传感器的测量值。P（a|b）称为a的后验分布，贝叶斯准则提供了一种由后验“逆概率”p（b|a）和先验概率p（a）来计算后验概率的方法。贝叶斯公式中的p（b）和a没有关系，通常可以测得，或者经常将写成归一化变量公式如下：

1.1置信度

置信度一般表示为bel()=p(|,)
上式中，x表示t时刻的x值或者一般表示机器人的状态，z表示t时刻的测量值，u是t时刻我们给机器人的控制，例如让它加速等等，因此置信度也就是在传感器测得z，并且执行u命令之后机器人实际状态是x的概率。

1.2贝叶斯滤波算法

贝叶斯滤波算法通常只有两步
第一步：

（最前面那个符号是积分，抱歉）
运用了全概率公式，算出的先验概率
第二步，用贝叶斯公式算出的后验概率：

式中归一化变量η用于将置信度进行归一化处理。

卡尔曼

X(k|k-1)=A X(k-1|k-1)+B U(k)
P(k|k-1)=A P(k-1|k-1) A’+Q
X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1))
Kg(k)= P(k|k-1) / (H P(k|k-1) + R)
P(k|k)=（I-Kg(k) H）P(k|k-1)

以上五个式子是卡尔曼算法的核心，x是置信度的均值，p为置信度的方差，u是控制量，z是传感器的数据，以上四个量都是向量

matlab仿真

创建200个均值为二十五的随机数
X(0|0)=1 ，P(0|0)=10。卡尔曼滤波和平滑滤波进行对比

clear
clc;
N=300;
CON = 25;
%%%%%%%%%%%%%%%kalman filter%%%%%%%%%%%%%%%%%%%%%%
x = zeros(1,N);
%randn(1,N)
y = 2^0.5 * randn(1,N)*0.5 + CON;%加过程噪声的状态输出
x(1) = 1;
p = 10;
cov(randn(1,N))
Q = cov(randn(1,N));%过程噪声协方差
R = cov(randn(1,N));%观测噪声协方差
for k = 2 : N
x(k) = x(k - 1);%预估计k时刻状态变量的值
p = p + Q;%对应于预估值的协方差
kg = p / (p + R);%kalman gain
x(k) = x(k) + kg * (y(k) - x(k));
p = (1 - kg) * p;
end
%%%%%%%%%%%Smoothness Filter%%%%%%%%%%%%%%%%%%%%%%%%
Filter_Wid = 10;
smooth_res = zeros(1,N);
for i = Filter_Wid + 1 : N
tempsum = 0;
for j = i - Filter_Wid : i - 1
tempsum = tempsum + y(j);
end
smooth_res(i) = tempsum / Filter_Wid;
end
% figure(1);
% hist(y);
t=1:N;
figure(1);
expValue = zeros(1,N);
for i = 1: N
expValue(i) = CON;
end
subplot(2,2,1),plot(t,expValue);
subplot(2,2,2),plot(t,x);
subplot(2,2,3),plot(t,y);
subplot(2,2,4),plot(t,smooth_res);
axis([0 N 20 30])

图一为理想值（25）
图二为卡尔曼输出的最优化结果
图三为高斯白噪音
图四为平滑滤波结果
侵删