卡尔曼滤波matlab仿真

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

这里不涉及具体的推导过程，站在巨人的肩膀上，直接使用结果：
卡尔曼滤波器的递归过程：
1) 估计时刻k 的状态：
X(k) = A*X(k-1) + B*u(k)
这里, u(k) 是系统输入

2) 计算误差相关矩阵P, 度量估计值的精确程度：
P(k) = A*P(k-1)*A’+ Q
这里, Q = E{ Wj^2 } 是系统噪声的协方差阵，即系统框图中的Wj的协方差阵, Q 应该是不断变化的，为了简化，当作一个常数矩阵。

3) 计算卡尔曼增益, 以下略去 (k), 即 P = P(k), X = X(k)：
K = P C’ (C * P * C’ + R) -1
这里 R = E{ Vj^2 }, 是测量噪声的协方差(阵), 即系统框图中的 Vj 的协方差, 为了简化，也当作一个常数矩阵。由于我们的系统一般是单输入单输出，所以 R是一个 1x1的矩阵，即一个常数，上面的公式可以简化为:
K = P C’ / (C P * C’ + R)

4) 状态变量反馈的误差量：
e = Z(k) – C*X(k)
这里的 Z(k) 是带噪声的测量量

5) 更新误差相关矩阵P
P = P – K * C * P

6) 更新状态变量：
X =X + K*e = X + K* (Z(k) – C*X(k))

7) 最后的输出：
Y = C*X

matlab 仿真：

%卡尔曼滤波实例
%测量房间温度，房间温度真实值为T=25度，一共测量两百个点
N=200;  T=25;  size=[N,1]; 
%取温度预测值的方差为Q=1e-3,温度传感器的测量方差为R=0.36，即我们更相信预测值，而较少相信传感器测量值。
Q=1e-3;  R=0.36;  T_mearsured=T+sqrt(R)*randn(size);
%初始时刻温度的最优估计值为T_start=22.5度,温度初始估计方差为P_start=2
T_start=22.5;  P_start=2;
T_kalman(1)=T_start;  P_kalman(1)=P_start;
%用_kalman的后缀表示最优估计值，用_pre的后缀表示预测值
for k=2:N
%在进行温度预测时，因为温度是一个连续的状态，我们认为上一时刻的温度和当前时刻的温度相等，则有T(k)=T(k-1)。
T_pre(k)=T_kalman(k-1);
P_pre(k)=P_kalman(k-1)+Q;
K(k)=P_pre(k)/(P_pre(k)+R);
T_kalman(k)=T_pre(k)+K(k)*(T_mearsured(k)-T_pre(k));
P_kalman(k)=P_pre(k)-K(k)*P_pre(k);
end
%画图
figure();
plot(T*ones(size),'g');
hold on
plot(T_mearsured,'b');
hold on
plot(T_kalman,'r');
legend('温度真实值','温度测量值','Kalman估计值')