带交互的卡尔曼滤滤波|一维滤波|源代码

背景

一维卡尔曼滤波的MATLAB例程，​背景为温度估计。

代码介绍

运行程序后，可以自己输入温度真实值：

以20℃为例，得到如下的估计值​：

滤波前的值和滤波后的值分别于期望值（真实值）作差，可以得到​误差​曲线图：​

误差统计特性

源代码

% 【PSINS】速度观测的153，EKF
% Evand/2024年4月2日/Ver1
% email：evandjiang@foxmail.com，微信 ：Evandworld（除前期达成一致外，咨询需付费）
clear;clc;close all;
rng(0);
glvs
psinstypedef(153);
trj = trjfile('trj10ms.mat');
% initial settings
[nn, ts, nts] = nnts(2, trj.ts);
% imuerr = imuerrset(0.03, 100, 0.001, 5);
imuerr = imuerrset(8, 14, 0.18, 57);
imu = imuadderr(trj.imu, imuerr);  % imuplot(imu);
davp0 = avperrset([1;1;10]*60, 0.1, [1;1;3]);

%% 速度观测EKF
ins = insinit(avpadderr(trj.avp0,davp0), ts);
rk = [1;1;1];
kf = kfinit(ins, davp0, imuerr, rk);
kf.Qt = diag([imuerr.web; imuerr.wdb; zeros(9,1)])^2;
kf.Rk = diag(rk)^2;
kf.Pxk = diag([davp0; imuerr.eb; imuerr.db]*1.0)^2;
kf.Hk = [zeros(3,3), eye(3), zeros(3,9)]; %修改观测方程
kf.Pmin = [avperrset(0.01,1e-4,0.1); gabias(1e-3, [1,10])].^2;  kf.pconstrain=1;
len = length(imu); [avp_EKF1, xkpk] = prealloc(fix(len/nn), 10, 2*kf.n+1);
% 完整代码的链接：https://gf.bilibili.com/item/detail/1105550012
timebar(nn, len, '15-state SINS/GPS Simulation.'); 
ki = 1;

更多关于导航、卡尔曼滤波的程序和讲解，见下方微信公众号