扩展卡尔曼滤波算法&Matlab仿真

chepey

已于 2024-06-17 12:16:00 修改

阅读量806

点赞数 23

分类专栏：算法仿真文章标签： matlab 笔记机器人数学建模信号处理信息与通信

于 2024-06-17 12:14:23 首次发布

本文链接：https://blog.csdn.net/qq_39627425/article/details/139740256

版权

算法仿真专栏收录该内容

1 篇文章 0 订阅

订阅专栏

扩展卡尔曼滤波

扩展卡尔曼滤波（EKF）是一种非线性的滤波方法。它继承了卡尔曼滤波算法数据融合的特点，也在系统线性度要求方面做出了改进。应用卡尔曼滤波时有以下两个缺一不可前提：

系统动态模型是线性的；

要求系统模型是线性的（包括状态转移方程和观测方程），保证了算法估计的全过程中：过程噪声和测量噪声始终保持高斯分布的特性
过程噪声和测量噪声都是高斯分布的白噪声。

卡尔曼滤波的本质是：利用两个正态（高斯）分布的融合仍是正态分布这一特性，进行最优迭代

扩展卡尔曼滤波在非线性系统中引入了近似估计的思想。对非线性模型在状态估计值附近作了1阶泰勒级数展开，得到了线性方程作为原状态方程和观测方程的近似表达形式（局部线性思想）。最后采用标准卡尔曼滤波进行估计。

非线性滤波方法

应用之中绝大多数都是非线性系统，此时需要使用非线性滤波方法。其中常用的有：

扩展卡尔曼滤波（EKF）
无迹卡尔曼滤波（UKF）
粒子滤波（PF）

标准卡尔曼滤波的五个公式&初始条件

公式

标准卡尔曼滤波的5个公式可以分为：2个预测公式+3个更新（修正）公式
$\\ (1)计算先验状态估计值（状态预测） \\ \hat{x}^-_k=A\hat{x}_{k-1}+Bu_k \\ (2)计算先验误差协方差（误差预测） \\ P^-_k=AP_{k-1}A^T+Q$

修正公式为：

$\\ (1)计算修正矩阵（Kalman增益计算） \\ K_k=P^-_kH^T(HP^-_kH^T+R)^{-1} \\ (2)更新观测值（状态修正，滤波结果） \\ \hat x_k=\hat x^-_k+K_k(z_k-H\hat x^-_k) \\ (3)更新误差协方差（误差更新） \\ P_k=(I-K_kH)P^-_k$

初始条件

算法输入的初始条件为：
$\hat x_0 初始状态\\ P_0 先验误差协方差矩阵\\ z_k观测输入\\ A状态转移矩阵\\ B控制转移矩阵\\ H观测转移矩阵\\ Q过程噪声协方差矩阵\\ R测量噪声协方差矩阵\\$

扩展卡尔曼滤波的五个公式、必要条件、同KLM区别

KLMvs.EKF

EKF更改KLM中的一些写法（线性-》非线性）

从线性模型的矩阵A、B、H的形式变为了状态函数f()和观测函数h()直接进行计算。
A、H矩阵由常数矩阵变为动态矩阵

未加粗为线性系统的状态和观测矩阵，加粗后为非线性系统函数的雅各比矩阵。

五个公式

扩展卡尔曼滤波的5个公式可以分为：2个预测公式+3个更新（修正）公式
$\\ (1)计算先验状态估计值（状态预测） \\ \hat{x}^-_k=f(\hat{x}_{k-1}) \\ (2)计算先验误差协方差（误差预测） \\ P^-_k=\mathbf{A}P_{k-1}\mathbf{A}^T+Q$

修正公式为：

$\\ (1)计算修正矩阵（Kalman增益计算） \\ K_k=P^-_k\mathbf{H}^T(\mathbf{H}P^-_k\mathbf{H}^T+R)^{-1} \\ (2)更新观测值（状态修正，滤波结果） \\ \hat x_k=\hat x^-_k+K_k(z_k-h(\hat x^-_k) \\ (3)更新误差协方差（误差更新） \\ P_k=(I-K_k\mathbf{H})P^-_k$

初始条件

初始条件与KLM算法相同，但转移矩阵从常数变成了函数矩阵。输入的初始条件为：
$\hat x_0 初始状态\\ P_0 先验误差协方差矩阵\\ z_k观测输入\\ \mathbf{A}状态转移矩阵（转移方程的雅各比矩阵）\\ \mathbf{H}观测转移矩阵（观测方程的雅各比矩阵）\\ Q过程噪声协方差矩阵\\ R测量噪声协方差矩阵\\$

仿真案例&Matlab代码

在这里插入图片描述

$如图所示，从空中水平抛射出的物体，初始水平速度v_x(0)，初始位置坐标(x(0),y(0))；\\ 受重力g和阻尼力影响，阻尼力与速度平方成正比，水平和垂直阻尼系数分别为k_x,k_y；还存在不确定\\ 的零均值白噪声干扰力\sigma a_x和\sigma a_y。在坐标原点处有一观测设备（不妨想象成雷达），可测的距离r\\(零均值白噪声误差\sigma_r)、角度\alpha（零均值白噪声误差\sigma_\alpha）。假设m=7.5kg，T为采样周期。$

准备工作

建立物体下落模型

设定状态向量

$\left[x_{(k)},v_{x(k)},y_{(k)},v_{y(k)} \right]$

非线性状态方程为：
$f({x_{(k)}},{v_{x(k)}},{y_{(k)}},{v_{y(k)}}):\left\{ \begin{array}{l} {x_{(k + 1)}} = {x_{(k)}} + {v_{x(k)}} \cdot T\\ {v_{x(k + 1)}} = {v_{x(k)}} + ( - {k_x}v_{x(k)}^2 + \delta {a_x}) \cdot T\\ {y_{(k + 1)}} = {y_{(k)}} + {v_{y(k)}} \cdot T\\ {v_{y(k + 1)}} = {v_{y(k)}} + ({k_y}v_{y(k)}^2 - g + \delta {a_y}) \cdot T \end{array} \right.$

设定观测向量

$\left[r,\alpha \right]$

非线性观测方程为：
$h({x_{(k)}},{v_{x(k)}},{y_{(k)}},{v_{y(k)}}):\left\{ \begin{array}{l} {r_{(k)}} = \sqrt {x_{(k)}^2 + y_{(k)}^2} + \delta r\\ {\alpha _{(k)}} = \arctan (\frac{x_{(k)}}{y_{(k)}}) + \delta \alpha \end{array} \right.$

计算状态转移函数和观测函数的雅各比矩阵

雅各比矩阵
$KaTeX parse error: Unknown column alignment: * at position 119: … \begin{array}{*̲{20}{c}} {\frac…$
状态转移矩阵为：
$KaTeX parse error: Unknown column alignment: * at position 28: …{\begin{array}{*̲{20}{c}} 1&T&0&…$
观测矩阵为：
$KaTeX parse error: Unknown column alignment: * at position 28: …{\begin{array}{*̲{20}{c}} {\frac…$

计算过程协方差Q和噪声协方差R

本小节内容参考如下链接：

扩展卡尔曼滤波算法——基本原理及举例（python实现radar数据滤波）_拓展卡尔曼滤波算法-CSDN博客

状态变量的选取中各变量的噪声相互独立，则过程协方差应为对角阵。这两个矩阵均为常数矩阵，本仿真中通过调节参数来确定。

Q:过程噪声的协方差，p(w)~N(0,Q)，噪声来自真实世界中的不确定性，N(0,Q) 表示期望是0，协方差矩阵是Q。Q中的值越小，说明预估的越准确

$KaTeX parse error: Unknown column alignment: * at position 28: …{\begin{array}{*̲{20}{c}} {cov(x…$

测量的协方差矩阵R，一般厂家给提供，R中的值越小，说明测量的越准确。

Q = [1e-11,0,0,0;
     0,1e-11,0,0;
     0,0,9e-4,0;
     0,0,0,1e-5];
R = [1e-2,0
     0,5e-1];

建立matlab框架

clc;clear;
%阻尼系数
kx = 0.05;
ky = 0.05;
%重力加速度
g = 9.8;

%初始状态[x0,vx,y0,vy]
vx0 = 1;
y0 = 500;
state_pred = [0,vx0,y0,0]';
state_pred_log = [];
P = zeros(4);
Q = [1e-11,0,0,0;
     0,1e-11,0,0;
     0,0,9e-4,0;
     0,0,0,1e-5];
R = [0.01,0
     0,0.5];

%采样周期/s
T = 0.01;
%噪声系数
err_K = 0.1;

%系统记录，记录真实运行状态
system_time = 0;
system_state = state_pred;
system_state_log = [];


while system_state(3)>0
    %记录上一次运行状态、预测状态
    system_state_log = [system_state_log ; system_state'];
    state_pred_log = [state_pred_log ; state_pred'];

    %%%%%%%%%%%%% 填入算法 %%%%%%%%%%%%%

    %%%%%%%%%%%%% 填入算法 %%%%%%%%%%%%%

    %系统状态更新
    system_state(1) = system_state(1)+system_state(2)*T;
    system_state(2) = system_state(2)+(-kx*system_state(2)^2+(-kx)*system_state(2)^2*err_K*randn(1))*T;
    system_state(3) = system_state(3)+system_state(4)*T;
    system_state(4) = system_state(4)+(-ky*system_state(4)^2-g+(-ky*system_state(4)^2-g)*err_K*randn(1))*T;
end

建立矩阵生成函数

function A = A_cal(kx,ky,vx,vy,Time)
%FUNC 计算状态转移矩阵
A = [1,Time,0,0;
     0,1-2*kx*vx*Time,0,0;
     0,0,1,Time;
     0,0,0,1+2*ky*vy*Time];
end

function H = H_cal(x,y)
%FUNC 计算观测矩阵
buf = sqrt(x^2+y^2);
buf2 = 1+(x/y)^2;
H = [x/buf   ,0,y/buf        ,0;
     1/y/buf2,0,(-x/y^2)/buf2,0];
end

填入核心算法

%计算雷达接收数据
r = sqrt(system_state(1)^2+system_state(3)^2);
alpha = atan(system_state(1)/system_state(3));
r = r + 0.01*r*randn(1);
alpha = alpha +0.01*alpha*randn(1);
z = [r,alpha];

%计算下一状态
A = A_cal(kx,ky,state_pred(2),state_pred(4),T);
H = H_cal(state_pred(1),state_pred(3));

state_pred = A*state_pred;
P = A*P*A'+Q;
Kk = P*H'*inv(H*P*H'+R);
state_pred = state_pred+Kk*(z - H*state_pred);
P = (eye(4)-Kk*H)*P;

整体代码

clc;clear;
%阻尼系数
kx = 0.05;
ky = 0.05;
%重力加速度
g = 9.8;

%初始状态[x0,vx,y0,vy]
vx0 = 1;
y0 = 500;
state_pred = [0,vx0,y0,0]';
state_pred_log = [];
P = zeros(4);
Q = [1e-11,0,0,0;
     0,1e-11,0,0;
     0,0,9e-4,0;
     0,0,0,1e-5];
R = [0.01,0
     0,0.5];

%采样周期/s
T = 0.01;
%噪声系数
err_K = 0.1;

%系统记录，记录真实运行状态
system_time = 0;
system_state = state_pred;
system_state_log = [];


while system_state(3)>0
    %记录上一次运行状态、预测状态
    system_state_log = [system_state_log ; system_state'];
    state_pred_log = [state_pred_log ; state_pred'];

    
    %计算雷达接收数据
    r = sqrt(system_state(1)^2+system_state(3)^2);
    alpha = atan(system_state(1)/system_state(3));
    r = r + err_K*r*randn(1);
    alpha = alpha +err_K*alpha*randn(1);
    z = [r,alpha];

    %计算下一状态
    A = A_cal(kx,ky,state_pred(2),state_pred(4),T);
    H = H_cal(state_pred(1),state_pred(3));

    state_pred = A*state_pred;
    P = A*P*A'+Q;
    Kk = P*H'*inv(H*P*H'+R);
    state_pred = state_pred+Kk*(z - H*state_pred);
    P = (eye(4)-Kk*H)*P;


    %系统状态更新
    system_state(1) = system_state(1)+system_state(2)*T;
    system_state(2) = system_state(2)+(-kx*system_state(2)^2+(-kx)*system_state(2)^2*err_K*randn(1))*T;
    system_state(3) = system_state(3)+system_state(4)*T;
    system_state(4) = system_state(4)+(-ky*system_state(4)^2-g+(-ky*system_state(4)^2-g)*err_K*randn(1))*T;


    %真实位置绘图
    plot(system_state(1),system_state(3),'*-','Color',[82,124,179]./255,'LineWidth',0.5,'MarkerSize',5)  
    hold on
    %预测位置绘图
    plot(state_pred(1),state_pred(3),'.-','Color',[66,64,71]./255,'LineWidth',0.5,'MarkerSize',10)  
    hold on
    %雷达读取散点
    plot(r*sin(alpha),r*cos(alpha),'o-','Color',[169,64,71]./255,'LineWidth',0.5,'MarkerSize',5)  
    hold on
end

lgd=legend('真实值','预测值','雷达读数');
lgd.Location='best';
lgd.FontSize=16;

title("EKF-初始x速度"+vx0+'初始高度'+y0,'FontSize',16);
tx = xlabel("时间(s)");
tx.FontSize=16;
ty = ylabel("距离(m)");
ty.FontSize=16;


ax=gca;grid on;box off
ax.LineWidth=2;
ax.Color=[249,250,245]./255;
ax.XMinorTick='on';
ax.YMinorTick='on';
ax.GridLineStyle='-.';
ax.XColor=[1,1,1].*.2;
ax.YColor=[1,1,1].*.2;


function A = A_cal(kx,ky,vx,vy,Time)
%FUNC 计算状态转移矩阵
A = [1,Time,0,0;
     0,1-2*kx*vx*Time,0,0;
     0,0,1,Time;
     0,0,0,1+2*ky*vy*Time];
end

function H = H_cal(x,y)
%FUNC 计算观测矩阵
buf = sqrt(x^2+y^2);
buf2 = 1+(x/y)^2;
H = [x/buf   ,0,y/buf        ,0;
     1/y/buf2,0,(-x/y^2)/buf2,0];
end