kalman滤波器C++设计仿真实例第三篇

1. 仿真场景

水面上有条船在做匀速直线航行,航行过程中由于风和浪的影响,会有些随机的干扰,也就是会有些随机的加速度作用在船身上,这个随机加速度的均方差大约是0.1,也就是说方差是0.01。船上搭载GPS设备,能够给出船的位置信息,目前GPS的定位误差为10m,也就它的定位方差是100。船的初始坐标为(-100, 200),水平两个方向的初始速度为(2, 20)。用kalman滤波器对船的航行位置等信息进行滤波估计。

2. 建模

这是一个平面运动的模型,先看一维情况。

这个跟上一个例子有些类似,不过就是没有确定的加速度,而只有随机的加速度而已。模型如下:

这里面的那个就是场景描述中说的那个随机干扰加速度。我们在状态方程里把它当成系统的随机噪声处理。把s和v作为状态量。注意,由于实际场景是平面二维的,所以实际的状态有四个,也就是四维的向量。

于是状态方程为:

系统量测方程就很好写了,只把GPS测量的位置信息作为量测量。

接下来确定Q和R。取GPS数据周期为1s,根据场景描述:

再看初值。根据场景描述。

P(0)不知道。就随便给个单位矩阵好了

检查一下,好像都齐了,那么开始仿真!

3. C++仿真程序

程序方面的话,只改Model那个类就好了,模型啥样照着写就行:

cpp 复制代码
//model.cpp
#include "model.h"


Model::Model(): Q(Matrix::unit(4)), R(100*Matrix::unit(2))
{
    double delta_w = 1e-2;
    Q(0, 0) = 0.5;
    Q(2, 2) = 0.5;
    Q = Q*delta_w;
}

Matrix Model::StateTrans(Matrix &X)
{
    double T = 1;
    Matrix F(Matrix::unit(4));
    F(0, 1) = T;
    F(2, 3) = T;
    return F*X;
}

void Model::StateUpdate(Matrix &X)
{
    X = StateTrans(X);
    X = X + Sqrtm(Q)*Matrix::randn(4, 1);
}

Matrix Model::MeasurPre(const Matrix &X)
{
    Matrix H(Matrix::zeros(2, 4));
    H(0, 0) = 1;
    H(1, 2) = 1;
    return  H*X;
}

Matrix Model::Measur(const Matrix &X)
{
    return MeasurPre(X) + Sqrtm(R)*Matrix::randn(2, 1);
}

Matrix Model::GetF()
{
    double T = 1;
    Matrix F(Matrix::unit(4));
    F(0, 1) = T;
    F(2, 3) = T;
    return F;
}

Matrix Model::GetH()
{
    Matrix H(Matrix::zeros(2, 4));
    H(0, 0) = 1;
    H(1, 2) = 1;
    return H;
}

然后是主程序:

cpp 复制代码
//main.cpp
#include "model.h"
#include "kalman.h"
#include <stdio.h>
int main()
{
    FILE *fp;
    fp = fopen("F:/data/data3.txt", "w");

    Model M;
    Matrix X(4);
    Matrix Z(2);
    X(0) = -100;
    X(1) = 2;
    X(2) = 200;
    X(3) = 20;

    Matrix P0(Matrix::unit(4));

    Kalman kf(4, 2);
    kf.Init(X, P0, &M);

    for(size_t i = 0; i < 120; i++)
    {
        M.StateUpdate(X);
        Z = M.Measur(X);
        kf.SetMeasur(Z);
        kf.Iterator();
        fprintf(fp, "%lf,%lf,%lf,%lf,%lf,%lf\n", X(0), X(2), kf.GetX()(0), kf.GetX()(2), Z(0), Z(1));
    }
    fclose(fp);
    cout << "done!";
    return 0;
}

只有初始化部分跟上一次的代码不一样。仿真120s,把仿真结果存到文件中。运行!ok!最后,把真实轨迹、测量轨迹和滤波后的轨迹放在一张图上比较一下:

红线是滤波后的轨迹,深蓝线是真实轨迹,浅蓝线是GPS轨迹。滤波后的轨迹当然与真实轨迹有些偏差,但比GPS输出的轨迹确实好太多了。

相关推荐
电科_银尘8 小时前
【大模型算法与工程 15】手写Self-Attention:Q、K、V与缩放点积
算法
我不会插花弄玉10 小时前
13.多路转接epoll【由浅入深-网络】
网络·c++·epoll
郝学胜-神的一滴11 小时前
Effective STL 条款03:容器中的对象复制要廉价且正确
开发语言·数据结构·c++·算法·rpc·stl
秋天的怀念_David11 小时前
算法一千零一夜 · 第三十三夜 · 跳表
java·程序人生·算法
西西弗Sisyphus12 小时前
C++ explicit 关键字工程实践
开发语言·c++
Pniubi14 小时前
力扣21合并两个有序链表
算法·leetcode·职场和发展
AIgorithmGEEK15 小时前
std::unordered_map 与 std::unordered_set:接口、算法技巧与权衡 (速查)
c++·算法·哈希表·负载因子·桶·unordered_map·unordered_set
上海潭龙东海科技有限公司16 小时前
智慧堆场动态模型三维实时孪生重构智能管控
大数据·网络·数据库·算法·重构
一拳一个呆瓜17 小时前
【STL】头文件:<cmath>
c++·stl
草莓熊Lotso17 小时前
【C++ 基于微服务的即时通讯系统】ElasticSearch 用户数据管理实现
linux·数据库·c++·elasticsearch·微服务·架构