目录
1.卡尔曼滤波简介
2.基本模型
2.1系统模型
2.2观测模型
3.预测与更新
3.1预测方程
3.2更新方程
4.matlab实现
4.1卡尔曼的5个方程说明
4.2一个状态估计
4.3两个状态估计
4.4三个状态估计
2、基本模型
卡尔曼滤波要做的是：
已知：
-
系统的初始状态 x0
-
每个时间的测量 Z
-
系统模型和测量模型
求解：
状态x随着时间变化而产生的值
2.1、系统模型
卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来，符合下式：
(1)
- Fk 是作用在 Xk−1 上的状态变换模型（/矩阵/矢量）。
- Bk 是作用在控制器向量uk上的输入－控制模型。
- Wk 是过程噪声，并假定其符合均值为零，协方差矩阵为Qk的多元正态分布。
(2)
2.2、测量模型
时刻k，对真实状态 xk的一个测量zk满足下式:
(3)
其中Hk是观测模型,它把真实状态空间映射成观测空间，vk 是观测噪声，其均值为零，协方差矩阵为Rk,且服从正态分布。
(4)
初始状态以及每一时刻的噪声{x0, w1, ..., wk, v1 ... vk} 都认为是互相独立的.
3、预测与更新
3.1、预测方程
预测是这样一个问题：
已知:
- 上一个状态的更新值
- 上一个状态的更新值和真实值之间的误差
求解：
- 这一个状态的预测值
- 这一个状态的预测值和真实值之间的误差
过程包括两个方面：
一、由上一个更新值 Xk-1|k-1 预测这一个预测值 Xk|k-1
二、由上一个更新值和真实值之间的误差 Pk-1|k-1 预测下一个预测值和真实值之间的误差 Pk|k-1
具体来说，就是以下两个方程。
(预测状态) (5)
(预测估计协方差矩阵) (6)
这里：
Xk-1|k-1 这种记法代表的是上一次的更新值，后面一个 k-1可以看做 Zk-1， 也就是上一次经过对比Zk-1(实际就是更新)之后所估计出的状态Xk-1。
Xk|k-1 这种记法代表这一次的预测值， 同理于刚才的介绍， 经过上一次Zk-1之后所估计出的状态Xk。
预测公式-预测状态
也就是公式(5)， 可以直接由系统模型导出。
预测公式-协方差矩阵：
P代表着估计误差的协方差，代表着一种 confidence ，比如先验估计误差(预测值与真实值之间误差)的协方差
3.2、更新方程
更新过程实际上就是一下问题：
已知：
- 由上一个更新值得到的当前的预测值。
- 当前的观测值
- 观测模型
求解：
- 融合了预测值和观测的更新值
- 由预测值的估计误差得到更新值的估计误差
更新方程如下：
其中K称为kalman增益， 就像一个补偿，决定着预测值应该变化多少幅度，才能变成更新值。
4、matlab实现
4.1、卡尔曼5个公式
4.2、一个状态方程
1 %本例子从百度文库中得到， 稍加注释 2 clear 3 N=200;%取200个数 4 5 %% 生成噪声数据 计算噪声方差 6 w=randn(1,N); %产生一个1×N的行向量，第一个数为0，w为过程噪声（其和后边的v在卡尔曼理论里均为高斯白噪声） 7 w(1)=0; 8 Q=var(w); % R、Q分别为过程噪声和测量噪声的协方差(此方程的状态只有一维，方差与协方差相同) 9 10 v=randn(1,N);%测量噪声 11 R=var(v); 12 13 %% 计算真实状态 14 x_true(1)=0;%状态x_true初始值 15 A=1;%a为状态转移阵，此程序简单起见取1 16 for k=2:N 17 x_true(k)=A*x_true(k-1)+w(k-1); %系统状态方程，k时刻的状态等于k-1时刻状态乘以状态转移阵加噪声（此处忽略了系统的控制量） 18 end 19 20 21 %% 由真实状态得到测量数据， 测量数据才是能被用来计算的数据， 其他都是不可见的 22 H=0.2; 23 z=H*x_true+v;%量测方差，c为量测矩阵，同a简化取为一个数 24 25 26 %% 开始 预测-更新过程 27 28 % x_predict: 预测过程得到的x 29 % x_update:更新过程得到的x 30 % P_predict:预测过程得到的P 31 % P_update:更新过程得到的P 32 33 %初始化误差 和 初始位置 34 x_update(1)=x_true(1);%s(1)表示为初始最优化估计 35 P_update(1)=0;%初始最优化估计协方差 36 37 for t=2:N 38 %-----1. 预测----- 39 %-----1.1 预测状态----- 40 x_predict(t) = A*x_update(t-1); %没有控制变量 41 %-----1.2 预测误差协方差----- 42 P_predict(t)=A*P_update(t-1)*A'+Q;%p1为一步估计的协方差，此式从t-1时刻最优化估计s的协方差得到t-1时刻到t时刻一步估计的协方差 43 44 %-----2. 更新----- 45 %-----2.1 计算卡尔曼增益----- 46 K(t)=H*P_predict(t) / (H*P_predict(t)*H'+R);%b为卡尔曼增益，其意义表示为状态误差的协方差与量测误差的协方差之比(个人见解) 47 %-----2.2 更新状态----- 48 x_update(t)=x_predict(t) + K(t) * (z(t)-H*x_predict(t));%Y(t)-a*c*s(t-1)称之为新息，是观测值与一步估计得到的观测值之差，此式由上一时刻状态的最优化估计s(t-1)得到当前时刻的最优化估计s(t) 49 %-----2.3 更新误差协方差----- 50 P_update(t)=P_predict(t) - H*K(t)*P_predict(t);%此式由一步估计的协方差得到此时刻最优化估计的协方差 51 end 52 53 %% plot 54 %作图，红色为卡尔曼滤波，绿色为量测，蓝色为状态 55 %kalman滤波的作用就是 由绿色的波形得到红色的波形， 使之尽量接近蓝色的真实状态。 56 t=1:N; 57 plot(t,x_update,'r',t,z,'g',t,x_true,'b');
4.3、两个状态方程
1 %本例子从百度文库中得到， 稍加注释 2 clear 3 N=20;%取200个数 4 dim=2; 5 sz=[dim,N]; 6 7 x(1)=0.1; 8 %系统模型 9 for k=2:20 10 x(k)=x(k-1)+0.1; 11 end 12 %% 生成噪声数据 计算噪声方差 13 % w=sqrt(0.0001)*randn(1,N); %产生一个1×N的行向量，第一个数为0，w为过程噪声（其和后边的v在卡尔曼理论里均为高斯白噪声） 14 % %w(1)=0; 15 % Q=var(w); % R、Q分别为过程噪声和测量噪声的协方差(此方程的状态只有一维，方差与协方差相同) 16 % 17 % v=sqrt(0.1)*randn(1,N);%测量噪声 18 % R=var(v); 19 20 q=0.00001; 21 Q=[q/3,q/2;q/2,q]; 22 R=0.1; 23 24 %% 计算真实状态 25 % x_true(1)=0;%状态x_true初始值 26 %F为状态转移阵，此程序简单起见取1 27 % for k=2:N 28 % x_true(k)=F*x_true(k-1)+w(k-1); %系统状态方程，k时刻的状态等于k-1时刻状态乘以状态转移阵加噪声（此处忽略了系统的控制量） 29 % end 30 F=[1,1;0,1]; 31 32 %% 由真实状态得到测量数据， 测量数据才是能被用来计算的数据， 其他都是不可见的 33 % H=0.2; 34 % z=H*x_true+v;%量测方差，c为量测矩阵，同a简化取为一个数 35 H=[1 0]; 36 y =[0.3 0.45 0.15 0.48 0.75 0.70 0.95 0.90 0.85 0.80 1.35 1.20 1.15 1.30 1.75 1.40 1.80 2.00 2.10 1.80 ]; 37 38 %% 开始 预测-更新过程 39 40 % x_predict: 预测过程得到的x 41 % x_update:更新过程得到的x 42 % P_predict:预测过程得到的P 43 % P_update:更新过程得到的P 44 45 % 对数组进行初始化 46 x_update=zeros(sz); % 对水位的后验估计。即在k时刻，结合浮标当前测量值与k-1时刻先验估计，得到的最终估计值 47 x_predict=zeros(sz); % 水位的先验估计。即在k-1时刻，对k时刻水位做出的估计 48 49 P_update=zeros(dim,dim,N); % 先验估计的方差 50 P_predict=zeros(dim,dim,N); % 后验估计的方差 51 K=zeros(sz); % 卡尔曼增益，反应了浮标测量结果与过程模型（即当前时刻与下一时刻浮标相同这一模型）的可信程度 52 % I=eye(dim);%单位阵 53 54 %初始化误差 和 初始位置 55 x_update(:,1)=[0 0]';%s(1)表示为初始最优化估计 56 P_update(:,:,1)=[1000 0;0,1000];%初始最优化估计协方差 57 58 for t=2:N 59 %-----1. 预测----- 60 %-----1.1 预测状态----- 61 x_predict(:,t) = F*x_update(:,t-1); %没有控制变量 62 %-----1.2 预测误差协方差----- 63 P_predict(:,:,t)=F*P_update(:,:,t-1)*F'+Q;%p1为一步估计的协方差，此式从t-1时刻最优化估计s的协方差得到t-1时刻到t时刻一步估计的协方差 64 65 %-----2. 更新----- 66 %-----2.1 计算卡尔曼增益----- 67 K(:,t)=P_predict(:,:,t)*H' / (H*P_predict(:,:,t)*H'+R);%b为卡尔曼增益，其意义表示为状态误差的协方差与量测误差的协方差之比(个人见解) 68 %-----2.2 更新状态----- 69 x_update(:,t)=x_predict(:,t) + K(:,t) * (y(t)-H*x_predict(:,t));%Y(t)-a*c*s(t-1)称之为新息，是观测值与一步估计得到的观测值之差，此式由上一时刻状态的最优化估计s(t-1)得到当前时刻的最优化估计s(t) 70 %-----2.3 更新误差协方差----- 71 P_update(:,:,t)=P_predict(:,:,t) - K(:,t)*H*P_predict(:,:,t);%此式由一步估计的协方差得到此时刻最优化估计的协方差 72 end 73 74 %% plot 75 %作图，红色为卡尔曼滤波，绿色为量测，蓝色为状态 76 %kalman滤波的作用就是 由绿色的波形得到红色的波形， 使之尽量接近蓝色的真实状态。 77 t=1:N; 78 plot(t,y,'r^-',t,x_update(1,t),'g^-',t,x,'b^-'); 79 legend('Mesured', 'Estimated', 'Ture Value'); 80 xl=xlabel('Time period'); 81 yl=ylabel('Level'); 82 % set(xl,'fontsize',FontSize); 83 % set(yl,'fontsize',FontSize); 84 set(gca,'color',[0.5,0.5,0.5]);
4.4、三个状态方程
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 设置初始化信息 % N：设置卡尔曼滤波器追踪点数 % r：设置估计变量个数，这里r=3 % s：被追踪的火箭的距离，初始值为1000m % v：火箭的速度，初始值为50m/s % a：火箭的加速度，初始值为20m/s2，此时加速度默认为不变 % T: 雷达的扫描间隔，此时设为1秒 % wt: 系统噪声，方差为20 % vt: 量测噪声，方差为16 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clear all; close all; N = 100; r = 3; t = 1:1:N; T = 1; s = zeros(1,N); v = zeros(1,N); a = zeros(1,N); a(t) = 20; s0 = 1000; v0 = 50; for n = 1:N v(n) = v0 + a(n)*n; s(n) = 1000+v0*n+0.5*a(n)*n*n; end wt = randn(1,N); wt = sqrt(4)*wt./std(wt); s = s + wt; v = v + wt; a(t) = a(t) + wt(t); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 卡尔曼滤波部分，继承之前初始化变量 % A：转移矩阵 % H：量测矩阵 % Qk：系统噪声矩阵 % Rk：量测噪声矩阵 % P0：均方误差矩阵初始值 % Y：火箭的状态矩阵，由k_s，k_v,k_a组成 % Y0：状态矩阵的初始值 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Y0 = [900 0 0]'; Y = [Y0 zeros(r,N-1)]; A = [1 1 0;0 1 1;0 0 1]; H = [1 0 0]; Qk = [0 0 0;0 0 0;0 0 20]; Rk = 16; P0 = [30 0 0;0 20 0;0 0 10]; P1 = P0; P2 = zeros(r,r); for k = 1:N Y(:,k) = A*Y(:,k); P2 = A*P1*A'+Qk; Kk = P2*H'*inv(H*P2*H'+Rk); Y(:,k+1) = Y(:,k)+Kk*(s(:,k)-H*Y(:,k)); P1 = (eye(r,r)-Kk*H)*P2; end k_s = Y(1,:); k_v = Y(2,:); k_a = Y(3,:); subplot(3,1,1); plot(t,s(t),'-',t,k_s(t),'o'); title('距离'); legend('实际值','估计值'); xlabel('t'); ylabel('s'); subplot(3,1,2); plot(t,v(t),t,k_v(t),'+'); title('速度'); legend('实际值','估计值'); xlabel('t'); ylabel('v'); subplot(3,1,3); plot(t,a(t),t,k_a(t),'-x'); title('加速度'); legend('实际值','估计值'); xlabel('t'); ylabel('a'); axis([0,N,0,30]);