网站建设工作怎么样,淘客联盟做任务网站,上海有哪些大型企业,青岛网站推广方案文章目录0.引言及友情链接1.场景预设2.扩展卡尔曼滤波器3.仿真及效果0.引言及友情链接 \qquad卡尔曼滤波器#xff08;Kalman Filter, KF#xff09;是传感器融合#xff08;Sensor Fusion#xff09;的基础#xff0c;虽然知乎、CSDN、GitHub等平台已有大量的学习资料Kalman Filter, KF是传感器融合Sensor Fusion的基础虽然知乎、CSDN、GitHub等平台已有大量的学习资料但还是建议大家在看完B站Matlab Tech Talk有关卡尔曼滤波器视频后再进入深入学习。友情链接提供如下
B站Matlab官方视频卡尔曼滤波器介绍CSDN初学者专用卡尔曼滤波器介绍知乎进阶篇扩展卡尔曼滤波器原理知乎适合入门扩展卡尔曼与无迹卡尔曼知乎进阶篇扩展卡尔曼滤波器公式推导Github.io
\qquad在此感谢各位辛勤的知乎、CSDN作者及B站分享视频的Up主。未学习卡尔曼滤波器的读者可学习链接中的1和2本文将介绍扩展卡尔曼滤波器Extended Kalman Filter, EKF的原理并以一个有关汽车运动的Matlab仿真说明其应用。 \qquad看过我上一篇介绍KF的博客的读者肯定知道KF设计的目的和结构即让状态估计的方差随时间推移趋于0然而由于KF针对的是随机系统这一点往往做不到而只能使其收敛为一个常数。EKF和KF的结构差不多只不过EKF针对的是非线性系统的滤波器。不含随机噪声的非线性系统状态方程如下 Nonlinear System: {x(k)f(x(k−1),u(k−1))y(k)g(x(k),u(k))\begin{cases} x(k)f(x(k-1),u(k-1))\\ y(k)g(x(k),u(k))\\ \end{cases} {x(k)f(x(k−1),u(k−1))y(k)g(x(k),u(k)) 引入EKF后其结构框图如下 \qquad其中x^\hat{x}x^为估计状态www为过程噪声一般由控制变量uuu引入但也可能由物理系统本身的不确定性引入而vvv为测量噪声。根据Matlab的帮助文档介绍噪声也分为两种——加入型噪声Additive Noise和非加入型噪声Nonadditive Noise其分类取决于噪声是否在非线性函数内部二者的状态方程形式如下 Additive Noise: {x(k)f(x(k−1),u(k))w(k)y(k)g(x(k),u(k))v(k)\begin{cases} x(k)f(x(k-1),u(k))w(k)\\ y(k)g(x(k),u(k))v(k)\\ \end{cases} {x(k)f(x(k−1),u(k))w(k)y(k)g(x(k),u(k))v(k) Nonadditive Noise {x(k)f(x(k−1),u(k),w(k))y(k)g(x(k),u(k),v(k))\begin{cases} x(k)f(x(k-1),u(k),w(k))\\ y(k)g(x(k),u(k),v(k))\\ \end{cases} {x(k)f(x(k−1),u(k),w(k))y(k)g(x(k),u(k),v(k)) 从上述表达式也可以看出加入型噪声是非加入型的特例。
1.场景预设
\qquad为了应用EKF需要构造一个非线性系统与前一篇讲述KF的博文保持连续性。这次使用的仍然所示一维的汽车运动模型状态变量仍然选择汽车的位移和速度(x[p,v]T)(x[p,v]^T)(x[p,v]T)但这次控制变量为u(k)u(k)u(k)为汽车的功率/汽车的质量重新构造状态方程如下 {x˙f(x,u)A0∗xB0∗Tu/(B0∗x)yg(x)12(C0∗x)T(C0∗x)\begin{cases} \dot{x}f(x,u)A_0^* xB_0^{*T}u/(B_0^*x)\\ yg(x)\frac{1}{2}(C_0^*x)^T(C_0^*x) \end{cases} {x˙f(x,u)A0∗xB0∗Tu/(B0∗x)yg(x)21(C0∗x)T(C0∗x) 其中A0∗[0100],B0∗C0∗[01]A_0^*\begin{bmatrix} 0 1\\0 0 \end{bmatrix},B_0^*C_0^*\begin{bmatrix}0 1\end{bmatrix}A0∗[0010],B0∗C0∗[01] 为了与控制变量uuu保持一致性此处的测量yyy为单位质量产生的动能。为了不失一般性添加非加入型噪声如下同样以yvy_vyv表示测量值yyy表示实际值y^\hat{y}y^表示估计值 {x˙f(x,u)A0∗xB0∗T(uw)/(B0∗x)yvg(x)12(C0∗x)T(C0∗x)v\begin{cases} \dot{x}f(x,u)A_0^* xB_0^{*T}(uw)/(B_0^*x)\\ y_vg(x)\frac{1}{2}(C_0^*x)^T(C_0^*x)v \end{cases} {x˙f(x,u)A0∗xB0∗T(uw)/(B0∗x)yvg(x)21(C0∗x)T(C0∗x)v 设定采样时间dtdtdt状态方程化为离散形式 {x(n)f(x(n−1),u(n−1))A0B0T(u(n−1)w)/(B0x(n−1))yv(n)g(x(n))12(C0x(n))T(C0x(n))v\begin{cases} x(n)f(x(n-1),u(n-1))A_0B_0^T(u(n-1)w)/(B_0x(n-1))\\ y_v(n)g(x(n))\frac{1}{2}(C_0x(n))^T(C_0x(n))v \end{cases} {x(n)f(x(n−1),u(n−1))A0B0T(u(n−1)w)/(B0x(n−1))yv(n)g(x(n))21(C0x(n))T(C0x(n))v 其中A0[1dt01],B0[01]A_0\begin{bmatrix} 1 dt\\0 1 \end{bmatrix},B_0\begin{bmatrix}0 1\end{bmatrix}A0[10dt1],B0[01] 与上一篇博文不同设E((B0w−B0w‾)T(B0w−B0w‾))Q,E((v−v‾)T(v−v‾))RE((B_0w-\overline{B_0w})^T(B_0w-\overline{B_0w}))Q,E((v-\overline{v})^T(v-\overline{v}))RE((B0w−B0w)T(B0w−B0w))Q,E((v−v)T(v−v))R
2.扩展卡尔曼滤波器
\qquad与上一篇博文一样本文不会从概率论或者最优化理论的角度对EKF的公式加以深入推导但会详细列出EKF最优估计的算法步骤。步骤会与Matlab的帮助文档的计算顺序略有出入但经过实验比较之后结果几乎没有差异。
设定初始状态变量的估计值x^0\hat{x}_0x^0并计算以下导数及P的初始值A0∂f∂x∣x^0,u0G0∂f∂w∣x^0,u0C0∂g∂x∣x^0S0∂g∂v∣x^0P0G0QG0TA_0 \frac{\partial f}{\partial x}|_{\hat{x}_0,u_0}\\[2ex]G_0\frac{\partial f}{\partial w}|_{\hat{x}_0,u_0}\\[2ex]C_0\frac{\partial g}{\partial x}|_{\hat{x}_0}\\[2ex]S_0\frac{\partial g}{\partial v}|_{\hat{x}_0}\\[2ex]P_0G_0QG_0^TA0∂x∂f∣x^0,u0G0∂w∂f∣x^0,u0C0∂x∂g∣x^0S0∂v∂g∣x^0P0G0QG0T令k1k1k1获取当前测量量yvy_vyv计算状态变量的先验估计x^k−f(x^k−1,uk−1)\hat{x}_k^-f(\hat{x}_{k-1},u_{k-1})x^k−f(x^k−1,uk−1)计算以下导数Ak∂f∂x∣x^k−,ukGk∂f∂w∣x^k−,ukCk∂g∂x∣x^k−Sk∂g∂v∣x^k−A_k \frac{\partial f}{\partial x}|_{\hat{x}_k^-,u_k}\\[2ex]G_k\frac{\partial f}{\partial w}|_{\hat{x}_k^-,u_k}\\[2ex]C_k\frac{\partial g}{\partial x}|_{\hat{x}_k^-}\\[2ex]S_k\frac{\partial g}{\partial v}|_{\hat{x}_k^-}\\[2ex]Ak∂x∂f∣x^k−,ukGk∂w∂f∣x^k−,ukCk∂x∂g∣x^k−Sk∂v∂g∣x^k−并顺带计算Pk−AkPAkTGkQGkTP_k^-A_kPA_k^TG_kQG_k^TPk−AkPAkTGkQGkT计算EKF的最优增益KkPk−CkT(CkPk−CkTSkRSkT)−1K_kP_k^-C_k^T(C_kP_k^-C_k^TS_kRS_k^T)^{-1}KkPk−CkT(CkPk−CkTSkRSkT)−1更新Pk(I−KkCk)Pk−P_k(I-K_kC_k)P_k^-Pk(I−KkCk)Pk−并计算状态变量的后验估计x^kx^k−Kk(yv−g(xk−))\hat{x}_k\hat{x}^-_kK_k(y_v-g(x^-_k))x^kx^k−Kk(yv−g(xk−))计算测量量的估计值y^kg(x^k)\hat{y}_kg(\hat{x}_k)y^kg(x^k)令kk1kk1kk1转步2
3.仿真及效果
仿真的Matlab代码如下
% 模拟要求汽车在一维空间的加速和减速过程
% 控制变量u是汽车的加速度
% 状态变量x[p,v],x^hat[v,a]
% w为控制变量的随机扰动v为测量的随机扰动
% Q为w的方差R为v的方差假设w与v相互独立
clear
dt 0.1; % 采样间隔
m 1e3; % 汽车自重
N 100; % 仿真数
Q 2e-4; % 过程噪声的协方差矩阵
R 0.01; % 测量噪声的方差
x0 [0;0.5]; % 初始位置和速度
xh0 [1;0.4]; % x0的估计
A0 [1,dt;0,1];
B0 [0,1];
f (x,u)(A0*xB0*dt*u./(B0*x)); % 系统方程的非线性函数
C0 sqrt([0,10]);
g (x,v)(1/2*(C0*x)*(C0*x)v); % 输出方程的非线性函数
A (x,u)(A0[0,0;0,-dt*u/x(2)^2]); % pf/px 2*2
G (x)([0;-dt/x(2)]); % pf/pw 2*1
C (x)(C0.*x); % pg/px 1*2
S 1; % pg/pv 2*1
P G(xh0)*Q*G(xh0); % 2*2
I eye(2);
u 0.01*ones(1,N); % 功率恒定 1*1
w sqrt(Q)*randn(1,N); % 控制变量的误差1*N
v sqrt(R)*randn(1,N); % 测量误差1*N
ye_list zeros(size(u)); % 估计值
yv_list zeros(size(u)); % 测量值
y_list zeros(size(u)); % 实际值
cov_list zeros(size(u)); % 测量方差
for i 1:numel(u)xreal f(x0,u(i)); % 真实的状态变量yreal g(x0,0); % 真实的测量x1 f(x0,u(i)w(i)); % 含噪声的状态变量 2*1yv g(x0,v(i)); % 含噪声的测量 1*1xfe f(xh0,u(i));Pfe A(xfe,u(i))*P*A(xfe,u(i))G(xfe)*Q*G(xfe);K Pfe*C(xfe)/(C(xfe)*Pfe*C(xfe)S*R*S); % 卡尔曼最优增益 2*1P (I - K*C(xfe))*Pfe; % 当前状态先验估计协方差xh1 xfeK*(yv-g(xfe,0)); % 状态预测ye g(xh1,0);x0 x1;xh0 xh1;y_list(i) yreal;yv_list(i) yv;ye_list(i) ye;cov_list(i) C(xh1)*P*C(xh1);
end
ax (1:N).*dt;
figure(1);
subplot(2,2,1)
plot(ax,y_list,ax,yv_list,ax,ye_list)
legend(实际,测量,估计,Location,best)
title(汽车的动能)
ylabel(动能/J)
xlabel(时间/s)
subplot(2,2,2)
plot(ax,yv_list-y_list,ax,ye_list-y_list)
legend(测量,估计,Location,best)
title(汽车的动能误差)
ylabel(动能/J)
xlabel(时间/s)
subplot(2,2,[3,4])
plot(ax,cov_list)
legend(测量方差,Location,best)
title(测量方差)
ylabel(方差/m^2)
xlabel(时间/s)\qquad这里设定的汽车的功率/质量为恒定的0.01汽车初速度为1m/s为恒功率加速过程根据能量守恒定律其测量量单位质量的动能应成近似线性增长。相关效果图如下
\qquad通过效果图可以看出虽然初始状态时估计值x^0\hat{x}_0x^0与真实值x0x_0x0相差较大造成EKF的误差较测量值较大但是经过一段时间推移后EKF的测量误差逐渐减小并较测量值有了显著提升。EKF算法对于非线性系统是近似收敛的从测量方差的走势也可以看出。需要指出的是这里的测量方差是单位质量的动能真实的动能需要乘上汽车的质量。 \qquad希望本文对您有帮助谢谢阅读。若有异议欢迎评论区指正
