前言
在上一篇【qt openGL入门笔记】OSG实现显示飞机模型飞行的点迹（二）-CSDN博客的基础上增加航迹显示功能。
效果图
代码结构变化
无新增的文件，OSGWidget不需要修改。
SituationCore
在数据层增加航迹类，知识点 α-β 滤波：预测→残差→修正
// ---------------- 航迹 ----------------
// 航迹 = 滤波器对目标状态的持续估计。对比 Plot（一次瞬时带噪观测），
// Track 是"这个目标现在在哪、往哪去"的稳定答案。
struct Track {
bool valid = false; // 是否已起始（收到第一个点迹后为 true）
osg::Vec3d pos; // 滤波位置（z恒0：雷达测不到高度）
osg::Vec3d vel; // 滤波速度（m/s）
double t = 0.0; // 本状态对应的时刻
int hits = 0; // 累计关联成功的点迹数 ≈ 航迹质量
};
// ---------------- α-β 航迹处理器 ----------------
// 每来一个点迹执行一轮"预测→关联→修正"。α-β 滤波是卡尔曼滤波
// 在匀速模型下的常增益简化版：增益固定、计算量极小。
// ---------------- α-β 航迹处理器 ----------------
class TrackProcessor {
public:
void setGain(double alpha, double beta) { m_alpha = alpha; m_beta = beta; }
void setGate(double g) { m_gate = g; }
void setMaxMissed(int n) { m_maxMissed = n; } // ★新增
int rejected() const { return m_rej; }
int missStreak() const { return m_miss; } // ★连续失联计数
void feed(const Plot& p)
{
const osg::Vec3d m = polarToWorld(p);
// 航迹起始（或失联重启后的重新起始）
if (!m_trk.valid) { init(m, p.t); return; }
const double dt = p.t - m_trk.t;
if (dt <= 0) return;
// ① 预测：匀速模型外推
const osg::Vec3d pred = m_trk.pos + m_trk.vel * dt;
const osg::Vec3d res = m - pred;
// ② 关联波门
if (res.length() > m_gate) {
++m_rej; ++m_miss;
// ★★ 关键修复：连续 maxMissed 个点迹都进不了波门，
// 判定原航迹已失效（目标急机动，匀速外推彻底跑偏），
// 用当前点迹重新起始航迹。
// 真实系统对应：航迹终止 + 未关联点迹起始新航迹。
// 没有这条，波门一旦拒收就死锁------预测永不修正，
// 残差每拍扩大 v·Δt·2sin(θ/2)≈1500m，再也进不了门。
if (m_miss >= m_maxMissed) init(m, p.t);
return;
}
m_miss = 0; // 关联成功，清零连续失联
// ③ 修正
m_trk.pos = pred + res * m_alpha;
m_trk.vel += res * (m_beta / dt);
m_trk.t = p.t;
m_trk.hits++;
}
// 漏测时的"滑行"保持（本场景不触发，但真实系统标配）
void missed(double t)
{
if (!m_trk.valid) return;
const double dt = t - m_trk.t;
m_trk.pos += m_trk.vel * dt;
m_trk.t = t;
}
const Track& track() const { return m_trk; }
private:
void init(const osg::Vec3d& m, double t) // ★ 起始逻辑提取成函数
{
m_trk.valid = true;
m_trk.pos.set(m.x(), m.y(), 0.0);
m_trk.vel.set(0, 0, 0);
m_trk.t = t;
m_trk.hits = 1;
m_miss = 0;
}
Track m_trk;
double m_alpha = 0.65;
double m_beta = 0.31;
double m_gate = 3000.0; // ★ 1500→3000：容纳急转弯后第一拍的~1500m残差
int m_rej = 0;
int m_miss = 0; // ★ 连续被拒计数
int m_maxMissed = 3; // ★ 连续3拍(1.5s)进不了门 → 重新起始
};SceneUtil
在渲染工具库中与航线有关的动态折线工具，效果图中的黄线。
// ============================================================
// 以下为过程3新增：动态折线工具
// ============================================================
// makeLine：动态折线（GL_LINE_STRIP），cap = 最大顶点数
inline osg::Geometry* makeLine(float width, const osg::Vec4& color, unsigned cap)
{
osg::ref_ptr<osg::Geometry> g = new osg::Geometry;
setupDynamic(g.get()); // 复用过程2的三件套
osg::ref_ptr<osg::Vec3Array> v = new osg::Vec3Array;
v->reserve(cap + 1);
g->setVertexArray(v.get());
g->addPrimitiveSet(new osg::DrawArrays(GL_LINE_STRIP, 0, 0)); // 初始0顶点
osg::ref_ptr<osg::Vec4Array> c = new osg::Vec4Array;
c->push_back(color);
g->setColorArray(c.get(), osg::Array::BIND_OVERALL);
osg::StateSet* ss = g->getOrCreateStateSet();
ss->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
ss->setAttributeAndModes(new osg::LineWidth(width));
ss->setAttributeAndModes(new osg::Depth(osg::Depth::ALWAYS)); // 信息层不被遮挡
return g.release();
}
// appendLinePoint：折线尾部加点（与 appendPoint 同一套 FIFO，
// 区别只在图元模式是 GL_LINE_STRIP）
inline void appendLinePoint(osg::Geometry* g, const osg::Vec3& p, unsigned cap)
{
auto* v = static_cast<osg::Vec3Array*>(g->getVertexArray());
if (v->size() >= cap) v->erase(v->begin());
v->push_back(p);
if (auto* da = dynamic_cast<osg::DrawArrays*>(g->getPrimitiveSet(0)))
da->set(GL_LINE_STRIP, 0, GLsizei(v->size()));
v->dirty();
}SituationScene
3D 态势图层中增加航迹图层。
class SituationScene
{
public:
void updateTrack(const situ::Track& trk); // ★ 过程3：航迹（有点迹时）
private:
void buildTracks(); // ★ 航迹图层
// 航迹图层（过程3）
osg::ref_ptr<osg::Geometry> m_trkLine; // 航迹历史线【增长式】
osg::ref_ptr<osg::Geometry> m_velLine; // 速度矢量【定长改写式】
osg::ref_ptr<osgText::Text> m_trkLabel; // 数据块
unsigned m_trkCap = 240;
};
static const double kVelLead = 58.0;
SituationScene::SituationScene(osg::Group* root, osg::Node* model)
: m_root(root)
{
buildTracks(); // ★ 新增图层
}
// ------------------------------------------------------------
// 图层4：航迹
// a) 航迹历史线：绿色，每次滤波输出追加一个点【增长式】
// b) 速度矢量：黄色，固定 2 顶点每拍改写【定长式】
// c) 数据块：T01 + 速度
// ------------------------------------------------------------
void SituationScene::buildTracks()
{
m_trkLine = su::makeLine(2.5f, osg::Vec4(0.25f, 1.f, 0.4f, 1.f), m_trkCap);
osg::ref_ptr<osg::Geode> g1 = new osg::Geode;
g1->addDrawable(m_trkLine.get());
m_root->addChild(g1.get());
m_velLine = su::makeLine(2.5f, osg::Vec4(1.f, 0.8f, 0.1f, 1.f), 2);
osg::ref_ptr<osg::Geode> g2 = new osg::Geode;
g2->addDrawable(m_velLine.get());
m_root->addChild(g2.get());
m_trkLabel = su::makeLabel(14.f, osg::Vec4(0.55f, 1.f, 0.65f, 1.f));
m_trkLabel->setText("T01");
m_root->addChild(su::labelGeode(m_trkLabel.get())); // 挂根节点 → 世界坐标
}
void SituationScene::updateTrack(const situ::Track& trk)
{
if (!trk.valid) return;
// 航迹在地面平面（雷达测不到高度）；z 抬 0.3 避免与网格线重叠
const osg::Vec3 p3(float(trk.pos.x()), float(trk.pos.y()), 0.3f);
const osg::Vec3 v3(float(trk.vel.x()), float(trk.vel.y()), 0.f);
// ① 航迹历史线：追加滤波位置（增长式 ------ 和点迹同一模式）
su::appendLinePoint(m_trkLine.get(), p3, m_trkCap);
// ② 速度矢量：当前位置 → 8 秒后预测位置。
// ★ 定长 2 顶点 → 首帧"占位"，之后只改写不增删。
// 这就是过程2预留的 rewritePoint 的用武之地：
// 数量固定的动态几何用改写，比 append+erase 更省。
const osg::Vec3 tip = p3 + v3 * float(kVelLead);
auto* v = static_cast<osg::Vec3Array*>(m_velLine->getVertexArray());
if (v->size() < 2) { // 首帧：占住 2 个顶点
v->push_back(p3);
v->push_back(tip);
if (auto* da = dynamic_cast<osg::DrawArrays*>(m_velLine->getPrimitiveSet(0)))
da->set(GL_LINE_STRIP, 0, 2);
v->dirty();
} else { // 之后：就地改写
su::rewritePoint(m_velLine.get(), 0, p3);
su::rewritePoint(m_velLine.get(), 1, tip);
}
// ③ 数据块：真实系统的数据块还有高度/机型/敌我属性，我们只有速度
char buf[64];
std::snprintf(buf, sizeof buf, "T01 %.0f m/s", trk.vel.length());
m_trkLabel->setText(buf);
m_trkLabel->setPosition(p3 + osg::Vec3(0.f, 0.f, 120.f));
// 注意：标签挂在根节点下 → 用【世界坐标】 setPosition，每拍更新。
// 对比 TRUTH 标签挂在 m_airMt 下 → 只设一次【本地偏移】。
// 两种坐标规则再次对照，这是过程1知识点的实战复现。
}MainWindow
MainWindow::onTick（30Hz）
├─ m_radar.step(t, plot) 出点迹时：
│ ├─ m_scene->addPlot(plot) → 红点
│ ├─ m_tracker.feed(plot) → 【数据层】起始/波门/α-β滤波
│ │ └─ 产出 Track{pos, vel, hits}
│ └─ m_scene->updateTrack(m_tracker.track()) → 绿线+黄矢量+数据块
└─ m_scene->updateTruth(t) → 飞机照飞（每帧）
situ::TrackProcessor m_tracker; // ★ 数据层：航迹处理器
MainWindow::MainWindow(const QString& modelFile, QWidget* parent)
: QMainWindow(parent)
{
......
m_radar.setNoise(80.0, 0.004);
m_tracker.setGain(0.65, 0.31); // ★ 显式设定增益，方便做对比实验
m_tracker.setGate(3000.0); // ★ 波门与机动残差的折中
}
void MainWindow::onTick()
{
m_simTime += m_dt;
// ① 雷达节拍：本拍可能出 0/1 个点迹
situ::Plot plot;
if (m_radar.step(m_simTime, plot)) {
m_scene->addPlot(plot); // 显示：红点
m_tracker.feed(plot); // ★ 数据：起始/波门/α-β滤波
m_scene->updateTrack(m_tracker.track()); // ★ 显示：绿线+黄矢量+数据块
}
// ② 飞机按真值飞（每帧，与雷达节拍独立）
m_scene->updateTruth(m_simTime);
}