卡尔曼滤波
1.简介
卡尔曼滤波简单来说，就是根据仪器的测量值以及上一时刻的预测值以及误差，来计算得到当前时刻的最佳预测值，从而一直递归得到每一时刻的预测值
2.基本模型
预测状态（预测值）:
一般忽略u：
xk：表示估计的状态
Ak：作用于xk-1的状态变换矢量矩阵
uk：控制输入，一般在实际应用忽略
Bk：作用于控制器向量uk的控制矩阵
wk：过程噪声（误差），符合正太分布，协方差矩阵为Qk=wwT
观测状态（测量值与状态的线性函数）：
zk：表示观测值
Hk：观测模型
vk：观测噪声（误差），符合正态分布，协方差矩阵为Rk=vvT
推导过程：
添加上标：xk上标表示忽略噪声情况下的先验状态估计
没有上标的表示后验估计值，也是要求的最后值
zk上标表示忽略测量噪声根据先验状态值得到的无噪声测量值
zk点上标表示预测测量值与实际测量值的差异，得到过程噪声和测量噪声对测量值的影响（噪声变量
经过整合推导得到后验状态估计值：
Kk为卡尔曼增益
后面求卡尔曼增益K~k~和P~k~（利用是后验与误差最小化的方式求得）
因此获得卡尔曼滤波的五条基本公式2.1，2.2，2.3，2.4，2.5
这五条公式形成一个递归，可以根据前面的计算算出后面的状态，对于公式中的未知参数定义如下：
u一般是忽略的
A,B,H对于每一个状态都是相同的，且大部分情况下是单位矩阵
R是测量的噪声协方差矩阵，可求
Q是过程噪声的协方差矩阵
z是测量值，根据仪器测量，是已知的
进入递归需要输入x0和P0，P0一般不取0
3.公式的推导
1）P~n+1~,n=FP~n,n~F^T^
2）测量方程z~n~=Hx~n~+v~n~
z~n~为测量的值，因此H只需要将x~n~的测量值在矩阵中设置为1便可
3）过程噪声的构造
x~n+1,n~=Fx~n,n~+Gu~n,n~+w~n~
分为离散噪声模型和连续噪声模型
1.离散噪声模型（位移，速度模型）
因此可以构建一个G
2.连续噪声模型
至于如何选取过程噪声模型以及方差的大小，如果构建的模型比较合理，方差设置小，但如果模型会受到很多影响，方差就得设置大；选择离散还是连续，最好进行尝试
4）卡尔曼增益推导
K~n~=P~n,n−1~H^T^(HP~n,n−1~H^T^+R~n~)^−1^
5）协方差更新方程
P~n,n~=(I−K~n~H)P~n,n−1~
6）所有公式
4.状态方程的推导（即F,G,H求解）
1）首先确定x（t）
2）然后根据系统关系，求出上述式子的A,B,C,D
3）然后通过A,B,C,D求出F,G
例子：
4.温度预测（一维系统）
假设一个系统
房间内两个相邻时刻的温度基本相同，差值标准差为0.02度
温度计的测量误差的标准差为0.5度
房间真实值为24度
设置初始温度为23.5度，误差的方差为1
五条核心公式如下：
xhatminus(k) = xhat(k-1); %用上一时刻的最优估计值来作为对当前时刻的温度的预测
Pminus(k) = P(k-1)+Q; %预测的方差为上一时刻温度最优估计值的方差与过程方差之和
% 测量更新（校正）
K(k) = Pminus(k)/( Pminus(k)+R ); %计算卡尔曼增益
xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k)); %结合当前时刻温度计的测量值，对上一时刻的预测进行校正，得到校正后的最优估计。该估计具有最小均方差
P(k) = (1-K(k))*Pminus(k); %计算最终估计值的方差
matlab仿真代码如下：
clear all;
close all;
% intial parameters
n_iter = 100; %计算连续n_iter个时刻
sz = [n_iter, 1]; % size of array. n_iter行，1列
x = 24; % 温度的真实值
Q = 4e-4; % 过程方差， 反应连续两个时刻温度方差。更改查看效果
R = 0.25; % 测量方差，反应温度计的测量精度。更改查看效果
z = x + sqrt(R)*randn(sz); % z是温度计的测量结果，在真实值的基础上加上了方差为0.25的高斯噪声。
% 对数组进行初始化
xhat=zeros(sz); % 对温度的后验估计。即在k时刻，结合温度计当前测量值与k-1时刻先验估计，得到的最终估计值
P=zeros(sz); % 后验估计的方差
xhatminus=zeros(sz); % 温度的先验估计。即在k-1时刻，对k时刻温度做出的估计
Pminus=zeros(sz); % 先验估计的方差
K=zeros(sz); % 卡尔曼增益，反应了温度计测量结果与过程模型（即当前时刻与下一时刻温度相同这一模型）的可信程度
% intial guesses
xhat(1) = 23.5; %温度初始估计值为23.5度
P(1) =1; %误差方差为1
for k = 2:n_iter
% 时间更新（预测）
xhatminus(k) = xhat(k-1); %用上一时刻的最优估计值来作为对当前时刻的温度的预测
Pminus(k) = P(k-1)+Q; %预测的方差为上一时刻温度最优估计值的方差与过程方差之和
% 测量更新（校正）
K(k) = Pminus(k)/( Pminus(k)+R ); %计算卡尔曼增益
xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k)); %结合当前时刻温度计的测量值，对上一时刻的预测进行校正，得到校正后的最优估计。该估计具有最小均方差
P(k) = (1-K(k))*Pminus(k); %计算最终估计值的方差
end
FontSize=14;
LineWidth=3;
figure();
plot(z,'k+'); %画出温度计的测量值
hold on;
plot(xhat,'b-','LineWidth',LineWidth) %画出最优估计值
hold on;
plot(x*ones(sz),'g-','LineWidth',LineWidth); %画出真实值
legend('温度计的测量结果', '后验估计', '真实值');
xl=xlabel('时间(分钟)');
yl=ylabel('温度');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
hold off;
set(gca,'FontSize',FontSize);
figure();
valid_iter = [2:n_iter]; % Pminus not valid at step 1
plot(valid_iter,P([valid_iter]),'LineWidth',LineWidth); %画出最优估计值的方差
legend('后验估计的误差估计');
xl=xlabel('时间(分钟)');
yl=ylabel('℃^2');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
set(gca,'FontSize',FontSize);
结果图为：
4.自由落体运动的位移预测
两个基本等式为
所以系统中状态向量包括位移与速度
状态方程为：
测量方程为：
其他固定值为：
因此可以使用matlab进行仿真
clc
clear all
close all
% 初始化参数
delta_t=0.1; %采样时间
t=0:delta_t:8;
N = length(t); % 序列的长度
sz = [2,N]; % 信号需开辟的内存空间大小 2行*N列 2:为状态向量的维数n
g=9.875; %加速度值
x=1/2*g*t.^2; %实际真实位置
z = x + sqrt(10).*randn(1,N); % 测量时加入测量白噪声
Q =[0 0;0 9e-1]; %假设建立的模型 噪声方差叠加在速度上 大小为n*n方阵 n=状态向量的维数
R = 10; % 位置测量方差估计，可以改变它来看不同效果 m*m m=z(i)的维数
A=[1 delta_t;0 1]; % n*n
B=[1/2*delta_t^2;delta_t];
H=[1,0]; % m*n
n=size(Q); %n为一个1*2的向量 Q为方阵
m=size(R);
% 分配空间
xhat=zeros(sz); % x的后验估计
P=zeros(n); % 后验方差估计 n*n
xhatminus=zeros(sz); % x的先验估计
Pminus=zeros(n); % n*n
K=zeros(n(1),m(1)); % Kalman增益 n*m
I=eye(n);
% 估计的初始值都为默认的0，即P=[0 0;0 0],xhat=0
for k = 9:N %假设车子已经运动9个delta_T了，我们才开始估计
% 时间更新过程
xhatminus(:,k) = A*xhat(:,k-1)+B*g;
Pminus= A*P*A'+Q;
% 测量更新过程
K = Pminus*H'*inv( H*Pminus*H'+R );
xhat(:,k) = xhatminus(:,k)+K*(z(k)-H*xhatminus(:,k));
P = (I-K*H)*Pminus;
end
figure
plot(t,z);
hold on
plot(t,xhat(1,:),'r-')
plot(t,x(1,:),'g-');
legend('含有噪声的测量', '后验估计', '真值');
xlabel('Iteration');
输出图表结果为：
若无法准确对状态进行准确的建模，我们应增加过程噪声来调整滤波的可靠性，以进行收敛
