Matlab 二阶Kalman

Second-Order-Kalman

拿垂直速度来说事情。
还是那次炸机日志。垂直速度和垂直高度Mission Planner中的数据如下：

一、输入测量速度估算高度和速度

1. 由速度建立模型得到高度，大概是下面这个样子，只有A阵没有B阵也没有噪声：
   

2. 写个获取数据函数：

function h = GetVerticalVel()

persistent VerticalVel k Init

if isempty(Init)
  load VerticalVel
  k = 1;
  Init = 1;
end
h = VerticalVel(k);
k = k + 1;

3. 二阶卡尔曼

function [pos vel] = SecondOrderKalman(z)

persistent A H Q R 
persistent x P
persistent Init

global AltToVel ;
if isempty(Init)
  Init = 1;
  % 大概680s，dt选择0.1s，由于这个dt不是实际输入故，最终计算的高度偏差会比较大。
  dt = 0.1;
  
  % 由预测状态方程得到A阵和B阵
  A = [ 1 dt;
        0 1  ];
 
 % 观测矩阵 是内部状态和输出之间的线性映射，列数 = 输出， 输入 = 函数
   if AltToVel == 1
       H = [1 0]; 
   else
       H = [0 1];
   end
 
  
  % Q R这里自己设置，看相信预测值还是测量值
  Q = [ 1 0;
        0 100 ];
    
  R = 10;
  x = [ 0 z ]';
  
  P = 5*eye(2);
end
% 系统二阶状态预测
xp = A*x;
% 系统二阶协方差预测
Pp = A*P*A' + Q;
% K阵
K = Pp*H'*inv(H*Pp*H' + R);
% 更新预测值
x = xp + K*(z - H*xp);
% 更新协方差阵
P = Pp - K*H*Pp;   

% 得到函数输出
pos = x(1);
vel = x(2);

4. 调用

clear all
global AltToVel;
AltToVel = 1;
Nsamples = 6800;
% 大概680s，dt选择0.1s，由于这个dt不是实际输入故，最终计算的高度偏差会比较大。
dt = 0.1;
t  = 0:dt:Nsamples*dt-dt;
% Nsample行 3列 矩阵用来保存数据
Xsaved = zeros(Nsamples, 3);

for k=1:Nsamples
  if AltToVel == 1
      z = GetAlt();  
  else
      z = GetVerticalVel();
  end
  [pos vel] = SecondOrderKalman(z);
  if AltToVel == 1
      Xsaved(k, :) = [pos -vel z];
  else
      Xsaved(k, :) = [-pos vel z];
  end
end

figure
hold on
plot(t, Xsaved(:, 1))
plot(t, Xsaved(:, 3), 'r.')
plot(t, Xsaved(:, 2), 'k')
legend('P', 'Measured', 'V')

5. 输出：
   由于时间是不准确的，模型也建的不好，还需要加速度也加进来。我这边是按照每个点是0.1s，6800个点，所以最后的结果还不准确，加点噪声也难补回来。
   

二、输入测量高度估算高度和速度

把全局变量 AltToVel改为1即可输入测量高度估算高度和速度。也就是改变下H阵即可
得到如下结果：

工程地址：https://gitlab.com/gkbytes/matlabfiltercoder/-/tree/master/Second-order%20Kalman