卡尔曼滤波kalman fiter

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。

组成以及步骤

1.预测状态方程

目的：由系统状态变量k-1时刻的最优值和系统输入计算出k时刻的系统预测值。

2.预测协方差方程

目的：根据k-1时刻的系统协方差预测k时刻系统协方差

3.卡尔曼增益方程

目的：根据（k时刻）协方差矩阵的预测值计算卡尔曼增益

4.跟新最优值方程（卡尔曼滤波的输出）

目的：根据状态变量的预测值和系统测量值计算出k时刻状态变量的最优值

5.更新协方差方程

目的：为了求k时刻的协方差矩阵。（为得到k+1时刻的卡尔曼输出值做准备）

代码实现

clc;clear

Z=1:100; % 观测值，测量值

noise=randn(1,100); % randn(m,n)返回一个m*n的随机项矩阵; randn(m,n)矩阵默认均值为0、方差为1的高斯噪声

Z=Z+noise; % 测量值含噪声（measurement noise），且为高斯白噪声

X=[0; 0]; % 初始状态

P=[1 0;0 1]; % 状态协方差矩阵

F=[1 1;0 1]; % 状态转移矩阵，理想的数学模型

% 加上外部不确定性后， 当前的每个预测状态都可能会移动到另一点，并且具有协方差Q

Q=[0.0001 0;0 0.0001]; % 状态转移协方差矩阵，预测误差

H=[1 0]; % 观测矩阵，对应C输出矩阵

R=1; % 传感器的观测噪声方差

figure;

hold on;

% 五步法

for i = 1:100

    %1状态预测

    X_ = F * X ; % 预测矩阵，得到机器人下一个状态X_ ；这里忽略了已知的外部影响Bu

    %2预测协方差矩阵

    %Cov(Ax)=A*Cov(x)*A'  ,更新下一个状态协方差矩阵P_

    P_ = F * P * F.' + Q ; % A的转置,rot90(A) 或 A' 或者A.'；这里不忽略外部的不确定性Q

    %3增益矩阵

    K = P_ * H.' / ( H * P_ * H.' + R);  % K卡尔曼增益系数

    %4状态最优估计，修正估计值

    X = X_ + K * ( Z(i) - H * X_ ); %卡尔曼滤波，更新下一个最佳估计状态X

    %5估计误差的协方差矩阵

    P = (eye(2)- K * H) * P_ ; %eye(2)为2阶单位阵，更新下一个最佳估计状态X对应的协方差矩阵P

    plot(X(1), X(2), 'r*-'); %横轴位置X(1)，纵轴速度X(2)

    plot(i, 1, 'gs');

end

% 卡尔曼滤波实质就是将预测状态量的高斯分布和观测量的高斯分布做融合，生成一个新的高斯分布，

% 其中新的高斯分布的均值和方差是两个独立的高斯分布的相关参量的加权，这个加权就是卡尔曼增益K，

% 但是预测状态量和观测量可能维度不同，需要将他们同时转换到一个向量空间中，所以观测量前面有线性变换矩阵