卡尔曼滤波示例

在上一篇卡尔曼滤波详解已经给出详细的推导过程。这里给出几个示例加深理解，这个是几年前学习卡尔曼滤波时候整理的，比较啰嗦，初学者可以看看，了解过的就不用浪费时间了，主要是最近在整理资料差点弄丢，还是放到网上保存吧。

简单示例1

这个示例中假设状态是稳定的，也就是真实的状态是不随着时间变化的，那么利用卡尔曼滤波算法最后会收敛到最优的状态。

参数设置

对于所有的变量，本场景下都是一个具体的数值而不是矩阵：

$u$ 是被忽略的；

$A=1$ 因为已经知道在该场景下，上一个状态和当前状态应该是相同的；

$H=1$ 一般来说测量值都是状态加上一些噪声，测量和要估计的状态是一致的，在实际应用都一般都是1；

${\hat{x}}_0=0$ 设置最初的状态为0，肯定不是真实的状态，但最后系统会收敛到真实状态；

$P_0=1$ 上面提到过不能设置为0，可以试一下，最后所有估计的状态都是0了；

$Q=0$ 和 $R=0.1$ 。

基本模型
$$\begin{array}{rl}{x}_k& =A{x}_{k-1}+B{u}_{k-1}+w_k\\ & =x_{k-1}\end{array}$$

$$\begin{array}{rl}{z}_k& =H{x}_k+v_k\\ & =x_k+v_k\end{array}$$

基本公式

Time Update	Measurement Update
$\overline{{\hat{x}}_k}={\hat{x}}_{k-1}$	$K_k=\overline{P_k}(\overline{P_k}+0.1{)}^{-1}$
$\overline{P_k}=P_{k-1}$	${\hat{x}}_k=\overline{{\hat{x}}_k}+K_k(z_k-\overline{{\hat{x}}_k})$
	$P_k=(1-K_k)\overline{P_k}$

仿真实验

给定测量值：

TIME(ms)	1	2	3	4	5	6	7	8	9	10
VALUE (V)	0.39	0.50	0.48	0.29	0.25	0.32	0.34	0.48	0.41	0.45

基本的卡尔曼滤波算法的代码：

z = [0, 0.39, 0.50, 0.48, 0.29, 0.25, 0.32, 0.34, 0.48, 0.41, 0.45];
s = size(z);
xp = [0]; Pp = [1]; xpp = [0]; Ppp = [1]; K = [0];
A = 1; H = 1; Q = 0; R = 0.1;
for k=2:s(2)
    xp(k) = A*xpp(k-1);
    Pp(k) = A*Ppp(k-1)*A' + Q;
    K(k) = Pp(k)*H/(H*Pp(k)*H + R);
    xpp(k) = xp(k) + K(k)*(z(k) - H*xp(k));
    Ppp(k) = (1 - K(k)*H)*Pp(k);
end

plot(1:s(2)-1, xpp(2:s(2)), 'r', 'LineWidth', 2);
axis([1 s(2)-1 0 0.5]);

下面是卡尔曼滤波算法运行过程中基本公式变量的变化：

$k$	$z_k$	$\overline{{\hat{x}}_k}$	$\overline{P_k}$	$K_k$	${\hat{x}}_k$	$P_k$
0					0	1
1	0.39	0	1	0.9091	0.3545	0.0909
2	0.50	0.3545	0.0909	0.4762	0.4238	0.0476
3	0.48	0.4238	0.0476	0.3226	0.4419	0.0323
4	0.29	0.4419	0.0323	0.2439	0.4049	0.0244
5	0.25	0.4049	0.0244	0.1961	0.3745	0.0196
6	0.32	0.3745	0.0196	0.1639	0.3656	0.0164
7	0.34	0.3656	0.0164	0.1408	0.3620	0.0141
8	0.48	0.3620	0.0141	0.1235	0.3765	0.0123
9	0.41	0.3765	0.0123	0.1099	0.3802	0.0110
10	0.45	0.3802	0.0110	0.0990	0.3871	0.0099

估计状态的效果图，可以看出来已经收敛了：

简单示例2

在本例子中，估计一个随机的标量常量，例如电压，真实的状态是不随着时间变化的，那么利用卡尔曼滤波算法最后会收敛到最优的状态。

参数设置

对于所有的变量，本场景下都是一个具体的数值而不是矩阵：

$u$ 是被忽略的；

$A=1$ 因为已经知道在该场景下， 上一时刻电压和当前电压应该是相同的（不考虑噪声的话）；

$H=1$ 电压表测量的值和要估计的值是相同的（不算噪声的话）；

${\hat{x}}_0=0$ 设置最初的状态为0，肯定不是真实的状态，但最后系统会收敛到真实状态；

$P_0=1$ 上面提到过不能设置为0，可以试一下，最后所有估计的状态都是0了，只要设置不为0结果就可以正确收敛；

$Q=1e-6$ 和在这里也可以设置为0，但设置一个非0小数值可以更灵活的来调整滤波，其实在该示例中生成的数据是没有过程噪声的；

$R=0.01$ 电压表模拟信号转化为数字信号不是完全精确有误差，假设有0.1的RMS白测量噪声，也就是标准差是0.1，那么方差为0.01。

基本模型
$$\begin{array}{rl}{x}_k& =A{x}_{k-1}+B{u}_{k-1}+w_k\\ & =x_{k-1}+w_k\end{array}$$

$$\begin{array}{rl}{z}_k& =H{x}_k+v_k\\ & =x_k+v_k\end{array}$$

基本公式

Time Update	Measurement Update
$\overline{{\hat{x}}_k}={\hat{x}}_{k-1}$	$K_k=\overline{P_k}(\overline{P_k}+0.01{)}^{-1}$
$\overline{P_k}=P_{k-1}+(1e-6)$	${\hat{x}}_k=\overline{{\hat{x}}_k}+K_k(z_k-\overline{{\hat{x}}_k})$
	$P_k=(1-K_k)\overline{P_k}$

仿真实验

在这里，设置要获得的标量常量 $x=-0.37727$ 。然后去模拟生成50个单独的测量数据 $z_k$ ，对每一个测量值随机生成测量噪声（服从均值为0，标准差为0.1的高斯分布）添加到上面设置的常量上生成测量值。同时可能需要做多组实验，预先生成测量集合可以保持每组实验数据是完全相同的。

生成数据代码如下，把生成的数据保存下来，后面运行卡尔曼滤波算法时不再重新生成，直接加载：

x = -0.37727;
num = 50;
mnoise = normrnd(0, 0.1, num, 1);
z = mnoise + x;
save('z.mat', 'z');

plot(1:num, repmat(x, [1, num]), 'k', 'LineWidth', 2);
hold on;
plot(1:num, z, 'b*');
maxz = max(z);
minz = min(z);
axis([0 num+1 minz-0.01 maxz+0.01]);
xlabel('Iteration');  
ylabel('Voltage');  
set(gca,'XTick',10:10:50)  
set(gca,'YTick',-0.6:0.1:-0.1)   

生成图如下，其中黑色线表示真实的状态值，而蓝色*表示添加白色高斯噪声之后的生成的测量值：

基本的卡尔曼滤波算法代码：

x = -0.37727;
num = 50;
mat = load('z.mat');
z = mat.z;

xp = [0]; Pp = [1]; xpp = [0]; Ppp = [1]; K = [0];
A = 1; H = 1; Q = 0.000001; R = 0.01;
for k=2:num+1
    xp(k) = A*xpp(k-1);
    Pp(k) = A*Ppp(k-1)*A' + Q;
    K(k) = Pp(k)*H/(H*Pp(k)*H + R);
    xpp(k) = xp(k) + K(k)*(z(k-1) - H*xp(k));
    Ppp(k) = (1 - K(k)*H)*Pp(k);
end

plot(1:num, repmat(x, [1, num]), 'k', 'LineWidth', 2);
hold on;
plot(1:num, z, 'b*');
plot(1:num, xpp(2:num+1), 'r', 'LineWidth', 2);

maxz = max(z);
minz = min(z);
axis([0 num+1 minz-0.01 maxz+0.01]);
xlabel('Iteration');  
ylabel('Voltage');  
set(gca,'XTick',10:10:50)  
set(gca,'YTick',-0.6:0.1:-0.1)  

% plot P versus iteration
figure;
plot(1:num, Ppp(2:num+1), 'k', 'LineWidth', 2);
xlabel('Iteration');  
ylabel('Pk');  
set(gca,'XTick',10:10:50)  
set(gca,'YTick',0.002:0.002:0.1)  

结果如下，可以看出来虽然测量值的噪声比较大，但是利用卡尔曼滤波算法还是很快收敛到正确的值，红色线是卡尔曼滤波算法估计的值：

然后上面考虑到对于 $P_0$ 的设置问题，提到只要设置为不为0的数值即可，最后结果也会正确的收敛。在下图中是 $P_k$ 的变化曲线，可以看出最后变为0.0002。

前面也提到过对于 $Q$ 和 $R$ 取值来说不是特别重要，最后结果都会收敛，但也说到，如果对这些噪声估计的更好，能得到更好的结果。

下面是把上面的 $R$ 设置为1得到的结果，该值变大了，说明测量误差较大，在估计过程中不会太相信这个测量值，所以最开始从初始值向下的波动，而没有太大的向测量值的波动：

下面是把上面的 $R$ 设置为0.0001得到的结果，该值变小了，说明测量误差较小，在估计过程中就会尽可能去相信这个测量值（包含噪声的测量值），所以会有偏向测量值的波动：

把这三个画在一张图上近距离观察一下，可以更好感受这个变化：

简单示例3

在本例子中，估计一个随机的标量变量，也就是说每个时间的状态是不一样的，例如飞机着陆时的海拔（当然也可以估计速度和燃料），那么利用卡尔曼滤波算法最后会估计每个时刻最优的状态。

参数设置

对于所有的变量，本场景下都是一个具体的数值而不是矩阵：

$u$ 是被忽略的；

$A=0.98$ 在该场景下，假设每个时刻飞机海拔下降2%，所以当前时刻飞机海拔是上一时刻飞机海拔的0.98（不考虑噪声的话）；

$H=1$ GPS或气压计等传感器获得的海拔和真实海拔是一致的（不算噪声的话）；

${\hat{x}}_0=1000$ 设置最初的状态为1000，最初的飞机海拔；

$P_0=1$ 上面提到过不能设置为0，可以试一下，最后所有估计的状态都是0了，只要设置不为0结果就可以正确收敛；

$Q=0$ 在该示例中生成的数据是没有过程噪声的；

$R=900$ 测量海拔的传感器提供不同程度的精度，标准差是30，那么方差为900，其实不用这是这么高，主要是防止噪声不明显看不出来效果。

基本模型
$$\begin{array}{rl}{x}_k& =A{x}_{k-1}+B{u}_{k-1}+w_k\\ & =0.98\ast x_{k-1}\end{array}$$

$$\begin{array}{rl}{z}_k& =H{x}_k+v_k\\ & =x_k+v_k\end{array}$$

基本公式

Time Update	Measurement Update
$\overline{{\hat{x}}_k}=0.98\ast {\hat{x}}_{k-1}$	$K_k=\overline{P_k}(\overline{P_k}+900{)}^{-1}$
$\overline{P_k}=0.98\ast P_{k-1}\ast 0.98$	${\hat{x}}_k=\overline{{\hat{x}}_k}+K_k(z_k-\overline{{\hat{x}}_k})$
	$P_k=(1-K_k)\overline{P_k}$

仿真实验

在这里，设置最初的飞机海拔为1000 ，其他时刻的真实海拔为上一时刻海拔的0.98。然后去模拟生成100个（再多的话没意义，都是0了）单独的测量数据 $z_k$ ，对每一个测量值随机生成测量噪声添加到该时刻的真实海拔上生成测量值。同时可能需要做多组实验，预先生成测量集合可以保持每组实验数据是完全相同的。

生成数据代码如下，把生成的数据保存下来，生成具有随机性，这里结果的数据见附件altitude_z1.mat，后面运行卡尔曼滤波算法时不再重新生成，直接加载：

x0 = 1000;
num = 100;
x = [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
end
mnoise = normrnd(0, 30, num, 1);
z = mnoise + x';
save('z.mat', 'z');

plot(1:num, z, 'g', 'LineWidth', 2);
hold on;
plot(1:num, x, 'k', 'LineWidth', 2);

数据分布图如下，其中黑色线表示真实的状态值，而绿色添加白色高斯噪声之后的生成的测量值：

基本的卡尔曼滤波算法代码：

x0 = 1000;
num = 100;
x = [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
end
mat = load('z.mat');
z = mat.z;

xp = [1000]; Pp = [1]; xpp = [1000]; Ppp = [1]; K = [0];
A = 0.98; H = 1; Q = 0; R = 900;
for k=2:num+1
    xp(k) = A*xpp(k-1);
    Pp(k) = A*Ppp(k-1)*A' + Q;
    K(k) = Pp(k)*H/(H*Pp(k)*H + R);
    xpp(k) = xp(k) + K(k)*(z(k-1) - H*xp(k));
    Ppp(k) = (1 - K(k)*H)*Pp(k);
end

plot(1:num, z, 'g', 'LineWidth', 2);
hold on;
plot(1:num, x, 'k', 'LineWidth', 2);
plot(1:num, xpp(2:num+1), 'r', 'LineWidth', 2);
xlabel('Iteration');  
ylabel('Altitude'); 

结果如下，可以看出来虽然测量值的噪声比较大，但是利用卡尔曼滤波算法还是很快收敛到正确的值，黑色线是真实的飞机海拔，红色线是卡尔曼滤波算法估计的海拔，可以看到黑色线几乎被全部遮挡住了，说明完美拟合了：

但是为什么效果会这么好呢，好的有点不符合预期，对上面的实验修改一下：

生成成新的数据但是添加测量噪声时是在当前时刻的真实飞机海拔上添加[-100, 100]内的一个随机数，不是高斯噪声了，增加噪声来看拟合效果，其他变量条件不变：

生成数据代码如下，生成具有随机性，这里结果的数据见附件altitude_z2.mat：

x0 = 1000;
num = 100;
x = [];
z= [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
    z(i) = x(i) + randi([-100, 100], 1, 1);
end
save('z.mat', 'z');

plot(1:num, z, 'g', 'LineWidth', 2);
hold on;
plot(1:num, x, 'k', 'LineWidth', 2);

数据分布图如下，其中黑色线表示真实的状态值，而绿色添加随机数噪声之后的生成的测量值，这时候的噪声已经很大了：

那么在执行上面的卡尔曼滤波算法，里面参数不变：

看上面的结果还能拟合的这么好，在这么大的噪声前提下，这说明啥问题，现在解释一下，主要有下面两方面原因：

第一是设置了一个正确的 $x_0$ ；

第二是设置了一个比较大的 $R$ ；

这会导致什么情况呢？卡尔曼滤波算法基本上会忽略掉测量值的作用，因为 $R$ 太大表示测量误差太大，系统不相信该值，在示例2中也讨论过，系统会尽可能相信过程中计算的飞机海拔，同时还提供了一个完美的 $x_0$ ，所以肯定完美的拟合了。

那么好在进行卡尔曼滤波算法时设置 $x_0=200$，终于不是完美的拟合了吧，明显红色是另一条函数曲线，所以目前仅仅是利用了过程计算的信息，测量的值几乎忽略掉了，当然本来也是当前的测量值误差也太大：

在该基础上调整 $R=1$ ， 可以看到下面效果要好很多了，主要是设置初始的 $x_0$ 差距太大，所以该示例也给以后使用提供了经验：

为什么说初始的 $x_0$ 会有这么大影响了，回到刚才的数据（添加高斯白噪声的），设置精确的 $R=900$ ，然后设置 $x_0=200$ ，得到下面的结果，虽然说 $R$ 设置是非常精确的，但是结果却不理想：

然后和上面一样调整 $R=1$ ， 可以看到下面效果要好很多了，主要是设置初始的 $x_0$ 差距太大，所以该示例也给以后使用提供了经验：

根据上面的测试以及示例2中的结果，如果设置的 $x_0$ 的值和真实相差太大，其实一般情况下的话，该值是根据场景自己来定义的，不会出现大的问题。但是在特殊情况下，如果该值的设置偏移比较大，那么就不能在进行卡尔曼滤波算法时设置太大的 $R$ 值，因为这种情况下，会导致测量噪声太大，系统不会相信提供的测量值，而仅仅是相信过程值，但是又因为过程值的初始值都是错误的，有很大的偏移，所以需要设置一个小的 $R$ 值，这样可以得到一个正确的估计。其实从常理上也可以推断，既然有测量值，就要尽可能的相信它，这其实是唯一的输入来源，所以 $R$ 不能设置特别大。在该示例中越小越好，可以把上面两组数据的 $R=0.0000001$ （不能设置为0） ，会发现效果特别好，几乎是完全拟合了，结果如下：

但还有种情况就是误差特别特别大，此时也不能设置的太小，不然的话会过于相信测量数据，在示例2中已经证明了。

但是看到上面的结果，误差其实也不小了，而且提供的初值偏移很大，还能这么好的被拟合，说明卡尔曼滤波算法太强大了。

然后对这两组数据调整一下 $P_0=0.001$ （设置为0效果一样），其他参数 $x_0=200$ ，$R=1$ ，结果如下，上面也提到该值如果太小，也会导致 $K_k$ 的值太小，所以在估计过程中不会去相信测量值：

上面也提到修改 $Q=1$ （设置太小都不会起作用） 可能会改变结果，但是手动的引入过程噪声，说明不能相信过程处理，那么在这种情况下，系统会极大的相信测量值，波动太大，如下：

所以根据上面对各个参数的设置调整实验，在实际使用中要尽可能设置一个合理的 $P_0$ ，然后为了更好的利用测量值，$R$ 的值不能设置太大也不能太小，尽可能合理。当然如果系统的初始量可以尽可能的设置正确是最好了，这样才能加快收敛过程。对于 $Q$ 一般来说是设置为0或者是很小的数，因为表明过程变化就是符合这个线性方程的，只是有很小的噪声扰动，设置太大，过程方程就利用不上了。而关于其他的参数 $A,B,H$ 就根据具体实际情况进行设置。

简单示例4

对示例3进行扩展，上面仅仅是估计飞机海拔，在这里估计飞机海拔(altitude)，飞机航向(heading)，飞机速度(airspeed)三个变量，然后测量仪器分别是用于测量海拔的气压计(barometer)和GPS（注意有两个测量仪器，需要融合），用于测量航向的罗盘(compass)，用于测量速度的皮托流速测定管(pitot tube)。

这种情况下会变得很复杂，基本模型中的测量方程如下：
$$\left[\begin{array}{c}{\text{barometer}}_k\\ {\text{compass}}_k\\ {\text{pitot}}_k\\ gp{s}_k\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 1& 0& 0\end{array}\right]\left[\begin{array}{c}{\text{altitude}}_{k-1}\\ {\text{heading}}_{k-1}\\ {\text{airspeed}}_{k-1}\end{array}\right]$$
本示例主要聚焦在传感器数据融合计算上，所以简化上面的模型，还是只顾及飞机海拔，但是利用气压计以及gps两个传感器的测量值来进行估计。

参数设置

对于所有的变量，本场景下都是都是以向量和矩阵的形式：

$u$ 是被忽略的；

$A=0.98$ 在该场景下，假设每个时刻飞机海拔下降2%，所以当前时刻飞机海拔是上一时刻飞机海拔的0.98（不考虑噪声的话）；

$H=\left[\begin{array}{c}1\\ 1\end{array}\right]$ 多个传感器融合；

${\hat{x}}_0=1000$ 设置最初的状态为1000，最初的飞机海拔；

$P_0=1$ 上面提到过不能设置为0，可以试一下，最后所有估计的状态都是0了，只要设置不为0结果就可以正确收敛；

$Q=0$ 在该示例中生成的数据是没有过程噪声的；

$R=\left[\begin{array}{cc}900& 0\\ 0& 900\end{array}\right]$ 测量海拔的传感器提供不同程度的精度，标准差是30，那么方差为900，气压计和GPS的噪声分布相同，但是独立分布的。

基本模型
$$\begin{array}{rl}{x}_k& =A{x}_{k-1}+B{u}_{k-1}+w_k\\ & =0.98\ast x_{k-1}\end{array}$$

$$\begin{array}{rl}{z}_k& =H{x}_k+v_k\\ & =\left[\begin{array}{c}1\\ 1\end{array}\right]\ast x_k+v_k\end{array}$$

基本公式

Time Update	Measurement Update
$\overline{{\hat{x}}_k}=0.98\ast {\hat{x}}_{k-1}$	$K_k=\overline{P_k}\left[\begin{array}{cc}1& 1\end{array}\right](\left[\begin{array}{c}1\\ 1\end{array}\right]\overline{P_k}\left[\begin{array}{cc}1& 1\end{array}\right]+\left[\begin{array}{cc}900& 0\\ 0& 900\end{array}\right]{)}^{-1}$
$\overline{P_k}=0.98\ast P_{k-1}\ast 0.98$	${\hat{x}}_k=\overline{{\hat{x}}_k}+K_k(z_k-\left[\begin{array}{c}1\\ 1\end{array}\right]\overline{{\hat{x}}_k})$
	$P_k=(1-K_k\left[\begin{array}{c}1\\ 1\end{array}\right])\overline{P_k}$

注意其中 $x$ 和 $P$ 都是标量，$z\in R^{2\times 1}$ 和 $K\in R^{1\times 2}$ 。

仿真实验

在这里，设置最初的飞机海拔为1000 ，其他时刻的真实海拔为上一时刻海拔的0.98。然后去模拟生成100个（再多的话没意义，都是0了）单独的测量数据 $z_k$ ，对每一个测量值随机生成测量噪声添加到该时刻的真实海拔上生成测量值，当然是对气压计和GPS单独模拟生成，同时可能需要做多组实验，预先生成测量集合可以保持每组实验数据是完全相同的。

生成数据代码如下，把生成的数据保存下来，生成具有随机性，这里结果的数据见附件altitude_z.mat ，后面运行卡尔曼滤波算法时不再重新生成，直接加载：

x0 = 1000;
num = 100;
x = [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
end
bnoise = normrnd(0, 30, num, 1);
gnoise = normrnd(0, 30, num, 1);
z = [bnoise + x',gnoise + x'];
save('z.mat', 'z');

plot(1:num, z(:, 1), 'g', 'LineWidth', 2);
hold on;
plot(1:num, z(:, 2), 'm', 'LineWidth', 2);
legend('barometer', 'gps')
plot(1:num, x, 'k', 'LineWidth', 2);

数据分布图如下，其中黑色线表示真实的状态值，而绿色添加白色高斯噪声之后的生成的气压计测量值，洋红色是添加白色高斯噪声之后的生成的GPS测量值：

基本的卡尔曼滤波算法代码：

x0 = 1000;
num = 100;
x = [];
for i=1:num
    x0 =  x0 * 0.98;
    x(i) = x0;
end
mat = load('z.mat');
z = mat.z;

xp = [1000]; Pp = [1]; xpp = [1000]; Ppp = [1]; K = [0 0];
A = 0.98; H = [1 1]'; Q = 0; R = [900 0;0 900];
for k=2:num+1
    xp(k) = A*xpp(k-1);
    Pp(k) = A*Ppp(k-1)*A' + Q;
    K = [K; Pp(k)*H'/(H*Pp(k)*H' + R)];
    xpp(k) = xp(k) + K(k, :)*(z(k-1, :)' - H*xp(k));
    Ppp(k) = (1 - K(k, :)*H)*Pp(k);
end

plot(1:num, z(:, 1), 'g', 'LineWidth', 2);
hold on;
plot(1:num, z(:, 2), 'm', 'LineWidth', 2);
legend('barometer', 'gps')
plot(1:num, x, 'k', 'LineWidth', 2);
plot(1:num, xpp(2:num+1), 'r', 'LineWidth', 2);
xlabel('Iteration');  
ylabel('Altitude'); 

结果如下，可以看出来虽然测量值的噪声比较大，但是利用卡尔曼滤波算法还是很快收敛到正确的值，黑色线是真实的飞机海拔，红色线是卡尔曼滤波算法估计的海拔，可以看到黑色线几乎被全部遮挡住了，说明完美拟合了：

和示例3一样设置 $x_0=200$ ，然后随后设置 $R=1,0.0000001$ 得到下面三个结果，和示例3结论一致：

简单示例5

再给出一个简单的小栗子，大家很熟悉的自由落体运动，在这里测量值只是针对高度。

参数设置

对于所有的变量，本场景下都是都是以向量和矩阵的形式：

$u$ 该部分是加速度部分，下面给出模型的时候会给出具体值；

$x$ 在该场景下表示需要估计物体的高度以及当前速度；

$A$ 和 $B$ 在下面后给出具体的值；

$H=\left[\begin{array}{cc}1& 0\end{array}\right]$ 在该场景下需要估计物体高度以及速度，但测量值只有高度，所以忽略速度项；

${\hat{x}}_0=\left[\begin{array}{c}100\\ 0\end{array}\right]$ 设置最初的物体的高度为100，速度为0；

$P_0=\left[\begin{array}{cc}1& 1\\ 1& 1\end{array}\right]$ 上面提到过不能设置为0，可以试一下，最后所有估计的状态都是0了，只要设置不为0结果就可以正确收敛；

$Q=0$ 在该示例中生成的数据是没有过程噪声的；

$R=1$ 测量高度误差项协方差。

基本模型

过程模型，包括高度（这里表示的是下降的高度，所以当前高度等于上一高度加上该时间段内上升的高度，在这里是下降，所以速度和加速度都是负值）以及速度：
$$\begin{gathered} y_k=y_{k-1}+v_{k-1}\Delta t+\frac{1}{2}a(\Delta t{)}^2 \\ {v}_k=v_{k-1}+a\Delta t \end{gathered}$$
所以把这两个状态值统一起来，（这里的 $v$ 表示速度不是测量误差）：
$$\begin{array}{rl}{x}_k& =\left[\begin{array}{c}{y}_k\\ v_k\end{array}\right]\\ & =\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]x_{k-1}+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]a\end{array}$$
而且在这里 $\Delta t=1$ 和 $a=-9.81$ ，所以上面的 $A=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]$ 和 $B=\left[\begin{array}{c}\frac{1}{2}\\ 1\end{array}\right]$

所以得到最终的模型：
$$x_k=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]x_{k-1}+\left[\begin{array}{c}\frac{1}{2}\\ 1\end{array}\right]\ast -9.81$$

$$\begin{array}{rl}{z}_k& =H{x}_k+v_k\\ & =\left[\begin{array}{cc}1& 0\end{array}\right]\ast x_k+v_k\end{array}$$

基本公式

Time Update	Measurement Update
$\overline{{\hat{x}}_k}=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]{\hat{x}}_{k-1}+\left[\begin{array}{c}\frac{1}{2}\\ 1\end{array}\right]\ast -9.81$	$K_k=\overline{P_k}\left[\begin{array}{c}1\\ 0\end{array}\right](\left[\begin{array}{cc}1& 0\end{array}\right]\overline{P_k}\left[\begin{array}{c}1\\ 0\end{array}\right]+1{)}^{-1}$
$\overline{P_k}=\left[\begin{array}{cc}1& 1\\ 0& 1\end{array}\right]P_{k-1}\left[\begin{array}{cc}1& 0\\ 1& 1\end{array}\right]$	${\hat{x}}_k=\overline{{\hat{x}}_k}+K_k(z_k-\left[\begin{array}{cc}1& 0\end{array}\right]\overline{{\hat{x}}_k})$
	$P_k=(I-K_k\left[\begin{array}{cc}1& 0\end{array}\right])\overline{P_k}$

注意其中 $x\in R^{2\times 1}$ ，$P\in R^{2\times 2}$ ，$K\in R^{2\times 1}$ 。

仿真实验

在这里，给出6组测量数据，如下：

TIME(ms)	1	2	3	4	5	6
HEIGHT(V)	127.0	115.3	110.9	72.4	50.7	0.3

基本的卡尔曼滤波算法的代码：

num = 6;
z = [ 127.0, 115.3, 110.9, 72.4, 50.7, 0.3];
x = [125];
xp = [100 0]'; Pp{1} = [1 1;1 1]; xpp = [100 0]'; Ppp{1} = [1 1;1 1]; K = [0 0]';
A = [1 1;0 1]; B = [1/2, 1]'; u = -9.81; H = [1 0]; Q = 0; R = 10;
for k=2:num
    x = [x x(1)-1/2*9.81*(k-1)^2];
    xp = [xp A*xpp(:, k-1) + B*u];
    Pp{k} = A*Ppp{k-1}*A' + Q;
    K = [K Pp{k}*H'/(H*Pp{k}*H' + R)];
    xpp = [xpp xp(:, k) + K(:, k)*(z(k) - H*xp(:, k))];
    Ppp{k} = (1 - K(:, k)*H)*Pp{k};
end

plot(0:num-1, z, 'g', 'LineWidth', 2);
hold on;
plot(0:num-1, x(1:num), 'k', 'LineWidth', 2);
plot(0:num-1, xpp(1,1:num), 'r', 'LineWidth', 2);
xlabel('height');  
ylabel('time'); 

这个例子和之前的比较类似，就不详细介绍了。