轻量级开源 C/C++ 扩展卡尔曼滤波器-TinyEKF

2025有所为

于 2025-05-13 16:42:50 发布

阅读量754

点赞数 13

分类专栏： EKF 文章标签：扩展卡尔曼滤波 EKF TinyEKF GPS

本文链接：https://blog.csdn.net/weixin_56029368/article/details/147915546

版权

EKF 专栏收录该内容

1 篇文章

订阅专栏

1 TinyEKF简介

TinyEKF是一个简洁的C/C++实现扩展卡尔曼滤波器，通用性足够适应不同的项目。为了能在Arduino、STM32和其他微控制器上实用，它使用静态（编译时）内存分配（不使用“new”或“malloc”）。examples文件夹包含了一个Arduino传感器融合的示例。python文件夹提供了一个抽象的Python类，可在实现C/C++版本之前用于原型设计你的EKF。c文件夹则包含了从文献中提取的一个纯C示例。除了类定义之外，python文件夹还包含了一个使用OpenCV进行鼠标追踪的例子。

2 下载和使用

github:GitHub - simondlevy/TinyEKF: Lightweight C/C++ Extended Kalman Filter with Python for prototypinggitcode:

https://gitcode.com/gh_mirrors/ti/TinyEKF

VSCode运行examples文件中的GPS示例

拷贝以下文件至文件夹GPS（新建的），其中gps.c为主程序文件、tinyekf.h包含矩阵运算相关实现函数、tinyekf_custom.h为EKF相关实现。data.csv为输入数据（25个历元，每个历元包含四个卫星的卫星位置和伪距信息）。

修改gps.c中的38行为#include "tinyekf.h"

生成tasks.json，修改如下：

{
    // See https://go.microsoft.com/fwlink/?LinkId=733558
    // for the documentation about the tasks.json format
    "version": "2.0.0",
	"tasks": [
		{
			"type": "shell",
			"label": "task g++",
			"command": "C://Mingw//bin//g++.exe",
			"args": [
				"-g",	//g++ -g
				"${file}",	//g++ -g main.cpp
				"-o",	//g++ -g main.cpp -o
				"${fileDirname}\\${fileBasenameNoExtension}.exe"	//g++ -g main.cpp -o main.exe
			],
			"options": {
				"cwd": "${workspaceFolder}"
			},
			"problemMatcher": [
				"$gcc"
			],
			"group": {
				"kind": "build",
				"isDefault": true
			},
			"presentation": {
				"panel": "shared"
			}
			
		}
	]
}

主要是“label”、“command”修改为mingw中g++.exe的路径。

生成launch.json，修改如下：

{
    // 使用 IntelliSense 了解相关属性。 
    // 悬停以查看现有属性的描述。
    // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387
    "version": "0.2.0",
    "configurations": [
        {
            "name": "(gdb) 启动",
            "type": "cppdbg",
            "request": "launch",
            "program": "${workspaceFolder}/GPS.exe",
            "args": [],
            "stopAtEntry": false,
            "cwd": "${fileDirname}",
            "environment": [],
            "externalConsole": true,
            "preLaunchTask": "task g++",
            "MIMode": "gdb",
            "miDebuggerPath": "C://Mingw//bin//gdb.exe",
            "setupCommands": [
                {
                    "description": "为 gdb 启用整齐打印",
                    "text": "-enable-pretty-printing",
                    "ignoreFailures": true
                },
                {
                    "description": "将反汇编风格设置为 Intel",
                    "text": "-gdb-set disassembly-flavor intel",
                    "ignoreFailures": true
                }
            ]
        }
    ]
}

主要是“externalConsole”、“preLaunchTask”、“miDebuggerPath”修改为mingw中gdb.exe的路径。

启动调试，编译成功，生成gps.exe可执行文件，运行后会输出滤波后的信息，保存在ekf.csv文件（每个历元接收机三维位置坐标和均值的差，类似于内符合定位精度）。

也可以在终端模式下运行gps.exe，打印输出程序运行的过程信息。

PS D:\Code\GPS> ./gps.exe
-2168832.046599 4386648.214558 4077172.040538
-2168842.841785 4386642.214172 4077173.870056
-2168843.744546 4386634.668619 4077171.952913
-2168843.742457 4386633.936942 4077169.866798
-2168840.351514 4386628.715729 4077167.463885
-2168839.619778 4386629.539772 4077162.256815
-2168838.139864 4386628.789142 4077160.520987
-2168837.432772 4386630.022548 4077153.928978
-2168837.981923 4386630.330923 4077149.492329
-2168836.230377 4386631.014996 4077148.314651
-2168835.019312 4386631.603244 4077148.329497
-2168837.009183 4386632.124964 4077149.212523
-2168836.705634 4386631.741776 4077151.144711
-2168835.865006 4386634.674140 4077148.979793
-2168837.991263 4386635.118928 4077146.292717
-2168837.733108 4386633.708690 4077151.856962
-2168837.660127 4386631.506054 4077155.242967
-2168837.468891 4386630.028514 4077153.616101
-2168837.442410 4386628.462408 4077156.958052
-2168838.108622 4386629.903104 4077155.022734
-2168837.461297 4386630.196597 4077155.138330
-2168837.575316 4386631.382120 4077152.899554
-2168837.721694 4386632.142844 4077151.405572
-2168839.291288 4386632.204313 4077154.409793
-2168839.402538 4386633.018873 4077153.767335
Wrote file ekf.csv
PS D:\Code\GPS>

3 EKF实现GPS位置求解代码gps_ekf分析

TinyEKF例程中的GPS位置求解，是参考gps_ekf: TinyEKF test case using You Chong's GPS example: http://www.mathworks.com/matlabcentral/fileexchange/31487-extended-kalman-filter-ekf--for-gps。将具体MATLAB算法通过C语言实现，为了更好的理解TinyEKF，可以先看gps_ekf的matlab代码。下载运行，结果如下图所示：

上图为GPS接收机三维坐标的扩展卡尔曼滤波和最小二乘法结果的对比，明显看到EKF更平滑，误差更小。接下来，详细看一下gps_ekf的算法设计。

根据GPS定位原理，伪距方程为： $rho = || X_s - X || + b + v$

其中，Xs 和 X 分别表示卫星和接收器的位置，|| Xs - X || 表示它们之间的距离。b 表示接收器的时钟偏差，需要与接收器的位置一起求解。rho 是接收器对每颗卫星的测量值（伪距），v 是伪距测量噪声，建模为白噪声。算法说明如下：

算法结构:

$[Xo,Po] = ExtendedKF(f,g,Q,R,Z,Xi,Pi)$

状态方程:

$X(n+1) = f(X(n)) + w(n)$

其中状态 $X$ 的维度为 $N\times 1$ ；

观测方程 :

$Z(n) = g(X(n)) + v(n)$

其中:

观测值 $Z$ 的维度为 $M\times 1$ ；

$w\sim N(0,Q)$ 是协方差为 $Q$ 的高斯噪声；

$v \sim N(0,R)$ 是协方差为 $R$ 的高斯噪声；

算法输入：

$f$ ：状态转移函数，它接受一个状态变量 $Xn$ ，并返回 $f(Xn)$ 和 $f$ 在 $Xn$ 处的雅可比矩阵。

作为一个假想的例子：

function [Val, Jacob] = f(X)
     Val = sin(X) + 3;
     Jacob = cos(X);
end

$g$ : 测量函数，它接收状态变量 $Xn$ 并返回 $g(Xn)$ 和 $g$ 在 $Xn$ 处的雅可比矩阵；

$Q$ : 过程噪声协方差矩阵，N×N ；

$R$ : 测量噪声协方差矩阵，M×M；

$Z$ : 当前测量值，M×1；

$Xi$ : 先验状态估计值，N×1；

$Pi$ ：先验估计状态协方差矩阵，N×N；
算法输出：

$Xo$ : 后验状态估计值，N×1；

$Po$ : 后验估计状态协方差矩阵，N×N维矩阵

算法流程：

扩展卡尔曼滤波算法：将输入函数 $f$ 和 $g$ 线性化，得到普通卡尔曼滤波的状态转移矩阵 $f_y$ 和观测矩阵 $H$ 。

状态方程:

$X(n+1) = f_y * X(n) + w(n)$

观测方程 :

$Z(n) = H * X(n) + v(n)$

具体算法步骤为：

$X_p = f(Xi)$ ：一步投影，线性化点；
$f_y=\frac{\mathrm{df} }{\mathrm{d} X}|_{X=X_p}$ ：线性化状态方程， $f_y$ 是过程模型的雅可比矩阵;
$H=\frac{\mathrm{dg} }{\mathrm{d} X}|_{X=X_p}$ ：线性化观测方程， $H$ 是测量模型的雅可比矩阵;
$P_p = f_y * P_i * f_y' + Q$ ：计算 $X_p$ 的协方差;
$K = P_p * H' * inv(H * P_p * H' + R)$ ：计算卡尔曼增益
$X_o = X_p + K * (Z - g(X_p))$ ：计算状态估计
$P_o = [I - K * H] * P_p$ ：计算 $X_o$ 的协方差

function [Xo,Po] = Extended_KF(f,g,Q,R,Z,Xi,Pi)
    N_state = size(Xi, 1);    
    [Xp, ~] = f(Xi);%1
    [~, fy] = f(Xp);%2
    [gXp, H] = g(Xp);%3
    Pp = fy * Pi * fy.' + Q;%4
    K = Pp * H' / (H * Pp * H.' + R);%5
    Xo = Xp + K * (Z - gXp);%6
    I = eye(N_state, N_state);
    Po = (I - K * H) * Pp;%7
end

3.1状态转移函数f(x)

假定接收机运动为匀速模型，需估计的状态量 $X$ 的维度为 $8\times 1$ ，即每次需估计8个未知量，依次为接收机x,y,z方向的坐标和速度（px,vx,py,vy,pz,vz）,clk（接收机钟差）,clk_drift（接收机钟飘，即钟差变化率）。

初始化接收机坐标为（-2.168816181271560e+006 ,4.386648549091666e+006, 4.077161596428751e+006），速度为（0,0,0）。钟差为3.575261153706439e+006(m)，钟飘为4.549246345845814e+001（m/s）；

根据接收机运动模型为匀速的假设，可得

$P(n+1)=P(n)+V*T$

$V(n+1)=V(n)$

假设接收机短时间内的钟飘稳定则，

$Clk(n+1)=Clk(n)+Clkdrift(n)*T$

$Clkdrift(n+1)=Clkdrift(n)$

可得 $f_y$ 雅可比矩阵为： $\begin{bmatrix} &1 &T &0 &0 &0 & 0 &0 &0 \\ &0 &1 &0 &0 &0 & 0 &0 &0\\ &0 &0 &1 &T &0 & 0 &0 &0 \\ &0 &0 &0 &1 &0 & 0 &0 &0 \\ &0 &0 &0 &0 &1 & T &0 &0 \\ &0 &0 &0 &0 &0 & 1 &0 &0 \\ &0 &0 &0 &0 &0 & 0 &1 &T \\ &0 &0 &0 &0 &0 & 0 &0 &1 \end{bmatrix}$ ，T为历元周期，本例中为1s。

具体实现如下：

X = zeros(8,1);
X([1 3 5]) = [-2.168816181271560e+006 
                    4.386648549091666e+006 
                        4.077161596428751e+006];                 %Initial position
X([2 4 6]) = [0 0 0];                                            %Initial velocity
X(7,1) = 3.575261153706439e+006;                                 %Initial clock bias
X(8,1) = 4.549246345845814e+001;                                 %Initial clock drift

f = @(X) ConstantVelocity(X, T);

% Constant Velocity model for GPS navigation.
function [Val, Jacob] = ConstantVelocity(X, T)
    Val = zeros(size(X));
    Val(1:2:end) = X(1:2:end) + T * X(2:2:end);
    Val(2:2:end) = X(2:2:end);
    Jacob = [1,T; 0,1];
    Jacob = blkdiag(Jacob,Jacob,Jacob,Jacob);
end

3.2 测量函数g(x)

根据GPS定位原理，每个卫星的伪距方程可表示为： $rho = || P_s - P || + clk + v$ ，其中，|| P_s - P || 表示，卫星和接收机的几何距离，b为接收机钟差clk，v为误差。

通常接收机得到多个卫星的观测量值即伪距rho和卫星的位置信息P_s ，通过求解方程的方式，得到接收机的坐标P和钟差b。如果我们得到当前时刻接收机的位置坐标估计Xp和钟差估计值clkp，再加上已知的卫星坐标P_s，就可以计算出当前时刻测量值的估计 $gX_p$ 。

$gX_p=|| P_s - X_P || + clk_P$

上式改写为 $g(X_P)=\sqrt{(PsX-X_PX)^{2}+(PsY-X_PY)^{2}+(PsZ-X_PZ)^{2}} +clk_P$

对上式的X方向求偏导得到

$\frac{\partial g}{\partial x}=\frac{PsX-X_PX}{\sqrt{(PsX-X_PX)^{2}+(PsY-X_PY)^{2}+(PsZ-X_PZ)^{2}} }$

其他方向同理可得。定义dx,dy,dz为卫星与接收机坐标的差（上式的分子），r为卫星与接收机的几何距离（上式的分母）。可得伪距测量模型的雅可比矩阵 $H$ 为：

$\begin{bmatrix} &dx1/r1 &0 &dy1/r1 &0 &dz1/r1 &0 &1 & 0\\ &dx2/r2 &0 &dy2/r2 &0 &dz2/r2 &0 &1 & 0 \\ &dx3/r3 &0 &dy3/r3 &0 &dz3/r3 &0 &1 & 0 \\ &dx4/r4 &0 &dy4/r4 &0 &dz4/r4 &0 &1 & 0 \end{bmatrix}$ ，

    % Set g: pseudorange equations for each satellites   
    g = @(X) PseudorangeEquation(X, SV_Pos{ii});

% Compute Val = || Xs - X || + b and its Jacobian.
function [Val, Jacob] = PseudorangeEquation(X, SV)

    % Each row of SV is the coordinate of a satellite.
    dX = bsxfun(@minus, X([1,3,5])', SV);% X - Xs
    Val = sum(dX .^2, 2) .^0.5 + X(7);
    Jacob = zeros(size(SV, 1), size(X, 1));
    Jacob(:, [1,3,5]) = bsxfun(@rdivide, dX, Val);
    Jacob(:, 7) = 1;
end

3.3 状态估计X的协方差矩阵P

初始化为对角阵，每个元素均为10；

P = eye(8)*10;

后续更新迭代。

Pp = fy * Pi * fy.' + Q;%4

3.4 测量噪声协方差矩阵R

初始化为对角阵，每个元素均为36；36为经验值，单位为m；

    % Set R
    Rhoerror = 36;    % variance of measurement error(pseudorange error)
    R = eye(size(SV_Pos{ii}, 1)) * Rhoerror;

3.5 过程噪声协方差矩阵Q

% Set Q, see [1]
Sf = 36;Sg = 0.01;sigma=5;         %state transition variance
Qb = [Sf*T+Sg*T*T*T/3 Sg*T*T/2;
	  Sg*T*T/2 Sg*T];
Qxyz = sigma^2 * [T^3/3 T^2/2;
                  T^2/2 T];
Q = blkdiag(Qxyz,Qxyz,Qxyz,Qb);

deepseek分析如下：

% 设置过程噪声协方差矩阵Q，参考[1]
Sf = 36; % 时钟零偏的功率谱密度（单位：m²/s³）
Sg = 0.01; % 时钟零偏随机游走的功率谱密度（单位：m²/s⁵）
sigma = 5; % 运动学状态的过程噪声标准差（单位：m/s²

Sf*T‌：零偏稳定性噪声的离散积分（相关时间内的累积效应，Sg*T^3/3‌：随机游走噪声的三阶项，体现零偏变化率的平方增长，Sg*T^2/2 表示零偏与零偏变化率的相关性；

运动学协方差矩阵 Qxyz

假设加速度为白噪声，对运动学方程进行两次积分：
- 位置方差 ∝ ∫∫a(t)dt ⇒ T³项
- 速度方差 ∝ ∫a(t)dt ⇒ T项
‌物理意义‌：
- T^3/3：位置噪声协方差（单位：m²）
- T：速度噪声协方差（单位：(m/s)²

3.6 完整代码

for ii = 1:N
    % Set g: pseudorange equations for each satellites   
    g = @(X) PseudorangeEquation(X, SV_Pos{ii});                              

    % Set R
    Rhoerror = 36;    % variance of measurement error(pseudorange error)
    R = eye(size(SV_Pos{ii}, 1)) * Rhoerror; 

    % Set Z
    Z = SV_Rho{ii}.';                   % measurement value

    [X,P] = Extended_KF(f,g,Q,R,Z,X,P);
    Pos_KF(:,ii) = X([1 3 5]).';       % positioning using Kalman Filter
end

SV_Pos为卫星的三维坐标、Z为伪距测量值。