卡尔曼滤波 学习记录

1.  卡尔曼滤波的5个公式

　   F:  状态转移矩阵

　   P: 状态协方差矩阵

　  Q:  状态转移协方差矩阵

      H:  观测矩阵

　  R:  观测噪声方差　

      K:  卡尔曼系数 滤波增益阵，它首先权衡预测状态协方差矩阵 P 与 观测值矩阵 R的大小，以此来觉得更倾向于相信预测模型还是详细观测模型。 

            如果相信预测模型多一点，那么这个残差的权重就会小一点。反之亦然，如果相信观察模型多一点，这个残差的权重就会大一点。不仅如此，

            滤波增益阵还负责把残差的表现形式从观测域转换到状态域。

  Xt ：初值设置；

           观测值已知的前两个点来确定。

 P的定义：状态协方差矩阵，P矩阵初值的设定公式如下：

 是径向距离的观测噪声方差，是方位角的观测噪声方差，这是P矩阵在极坐标系下的表示；

 R的定义：观测噪声方差 ；在匀速直线运动中，如果是极坐标系下的话，R矩阵的形式是：

matlab 算法测试代码： 

Z =(1:100);    %  观测值 
noise = randn(1,100);     % 方差为1的高斯噪声
Z = Z + noise ;

X =[0;1];      %  状态  (观测值已知的前两个点来确定)
P =[1 3;3 5];  %  状态协方差矩阵  (正对角线的是两个维度的方差，反对角线两个值是相等的，是他们的协方差)
F =[1 1;0 1];  %  状态转移矩阵 (描述了给定初始状态的状态如何随时间传播,对于线性时不变（LTI）系统，这是一个常数矩阵)
Q =[0.00001 0;0 0.0001];   %状态转移协方差矩阵 （认为很小）
H =[1 0];      %  观测矩阵
R = 1;         %  观测噪声方差 

figure ;
hold on ;

for i =1:100 
    % 预测值  
    X_= F * X ;
    P_= F * P * F'+ Q;      % 噪声协方差矩阵传递 + Q 协方差本身带来的噪声
    
    K = P_* H'/(H* P_* H'+ R); % 卡尔曼系数  滤波增益阵，它首先权衡预测状态协方差矩阵 P 与 观测值矩阵 R的大小，以此来觉得更倾向于相信预测模型还是详细观测模型。
                               % 如果相信预测模型多一点，那么这个残差的权重就会小一点。反之亦然，如果相信观察模型多一点，这个残差的权重就会大一点。不仅如此，滤
                               % 波增益阵还负责把残差的表现形式从观测域转换到状态域。
    X = X_+ K *(Z(i)-H * X_);
    P =( eye(2)- K * H)* P_;
        
    plot(X(1),X(2),'r*');      % 画点，横轴表示位置，纵轴表示速度
 end