文档章节

利用RK4更新四元數並轉換成歐拉角及 OpenGL 的 Viewer

cctsao1008
 cctsao1008
发布于 2014/11/28 11:56
字数 2306
阅读 270
收藏 1
C

開發平台 :

硬體規格

Odroid-U2

作業系統

Ubuntu 13.10

Codename: saucy

下面都是一些基本的程式可示需求修改, 之後我會把算法跟GUI整再一起開發.

四元數更新 :

Windows : dev-c++ 專案 : quat  ( 需安裝  dev-c++

Linux : GNU GCC

quat.c

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#define M_PI 3.14159265358979323846
//#define M_PI 3.14
#define D2R(x) ((x) * M_PI/180)
#define R2D(x) ((x) * 180/M_PI)
#define NUM 4
void RK4(float X[NUM], float U[NUM], float dT);
void StateEq(float X[NUM], float U[NUM], float Xdot[NUM]);
/* run this program using the console pauser or add your own getch, system("pause") or input loop */
// gcc quat.c -o quat -lm -std=c99
int main(int argc, char *argv[]) {
    float x[3] = {0};
    float q[4] = {1,0,0,0};
    float w[4] = {0,0,0,0};
    //float r[4] = {0,0,0,0};
    float m11, m12, m13, m23, m33;
    float dt = 0.000001;
    float norm;

    printf("x0=%10f, x1=%10f, x2=%10f \n", R2D(x[0]), R2D(x[1]), R2D(x[2]));

    // X
    printf("---X\n");
    w[1] = D2R(100), w[2] = 0, w[3] = 0;
    for(int i=0; i < 10; i++) {
        //r[0] = -w[1]*q[1] - w[2]*q[2] - w[3]*q[3];
        //r[1] = +w[1]*q[0] - w[2]*q[3] + w[3]*q[2];
        //r[2] = +w[1]*q[3] + w[2]*q[0] - w[3]*q[1];
        //r[3] = -w[1]*q[2] + w[2]*q[1] + w[3]*q[0];

        //q[0] = q[0] + (dt/2)* r[0];
        //q[1] = q[1] + (dt/2)* r[1];
        //q[2] = q[2] + (dt/2)* r[2];
        //q[3] = q[3] + (dt/2)* r[3];
        RK4(q,w,dt);

#if 1 // 不做的話會出現NAN 
        norm = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
        q[0] = q[0] / norm;
        q[1] = q[1] / norm;
        q[2] = q[2] / norm;
        q[3] = q[3] / norm;
#endif

        m11 = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
        m12 = 2.0 * (q[1]*q[2] + q[0]*q[3]);
        m13 = 2.0 * (q[1]*q[3] - q[0]*q[2]);
        m23 = 2.0 * (q[2]*q[3] + q[0]*q[1]);
        m33 = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];


        x[0] = atanf(m23/m33);
        x[1] = -asinf(m13);
        x[2] = atanf(m12/m11);

        printf("x0=%10f, x1=%10f, x2=%10f \n", R2D(x[0]), R2D(x[1]), R2D(x[2]));
    }

    // Y
    printf("---Y\n");
    w[1] = 0, w[2] = D2R(100), w[3] = 0;
    for(int i=0; i < 10; i++) {
        //r[0] = -w[1]*q[1] - w[2]*q[2] - w[3]*q[3];
        //r[1] = +w[1]*q[0] - w[2]*q[3] + w[3]*q[2];
        //r[2] = +w[1]*q[3] + w[2]*q[0] - w[3]*q[1];
        //r[3] = -w[1]*q[2] + w[2]*q[1] + w[3]*q[0];

        //q[0] = q[0] + (dt/2)* r[0];
        //q[1] = q[1] + (dt/2)* r[1];
        //q[2] = q[2] + (dt/2)* r[2];
        //q[3] = q[3] + (dt/2)* r[3];
        RK4(q,w,dt);

#if 1 // 不做的話會出現NAN 
        norm = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
        q[0] = q[0] / norm;
        q[1] = q[1] / norm;
        q[2] = q[2] / norm;
        q[3] = q[3] / norm;
#endif

        m11 = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
        m12 = 2.0 * (q[1]*q[2] + q[0]*q[3]);
        m13 = 2.0 * (q[1]*q[3] - q[0]*q[2]);
        m23 = 2.0 * (q[2]*q[3] + q[0]*q[1]);
        m33 = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];


        x[0] = atanf(m23/m33);
        x[1] = -asinf(m13);
        x[2] = atanf(m12/m11);

        printf("x0=%10f, x1=%10f, x2=%10f \n", R2D(x[0]), R2D(x[1]), R2D(x[2]));
    }

    // Z
    printf("---Z\n");
    w[1] = 0, w[2] = 0, w[3] = D2R(100);
    for(int i=0; i < 10; i++) {
        //r[0] = -w[1]*q[1] - w[2]*q[2] - w[3]*q[3];
        //r[1] = +w[1]*q[0] - w[2]*q[3] + w[3]*q[2];
        //r[2] = +w[1]*q[3] + w[2]*q[0] - w[3]*q[1];
        //r[3] = -w[1]*q[2] + w[2]*q[1] + w[3]*q[0];

        //q[0] = q[0] + (dt/2)* r[0];
        //q[1] = q[1] + (dt/2)* r[1];
        //q[2] = q[2] + (dt/2)* r[2];
        //q[3] = q[3] + (dt/2)* r[3];
        RK4(q,w,dt);

#if 1 // 不做的話會出現NAN 
        norm = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
        q[0] = q[0] / norm;
        q[1] = q[1] / norm;
        q[2] = q[2] / norm;
        q[3] = q[3] / norm;
#endif

        m11 = q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3];
        m12 = 2.0 * (q[1]*q[2] + q[0]*q[3]);
        m13 = 2.0 * (q[1]*q[3] - q[0]*q[2]);
        m23 = 2.0 * (q[2]*q[3] + q[0]*q[1]);
        m33 = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];


        x[0] = atanf(m23/m33);
        x[1] = -asinf(m13);
        x[2] = atanf(m12/m11);

        printf("x0=%10f, x1=%10f, x2=%10f \n", R2D(x[0]), R2D(x[1]), R2D(x[2]));
    }

    return 0;
}

//  *************  RungeKutta **********************
//  Does a 4th order Runge Kutta numerical integration step
//  Output, Xnew, is written over X
//  NOTE the algorithm assumes time invariant state equations and
//    constant inputs over integration step
//  ************************************************

void RK4(float X[NUM], float U[NUM], float dT)
{

    float dT2 = dT / 2, K1[NUM], K2[NUM], K3[NUM], K4[NUM], Xlast[NUM];
    int i;

    for (i = 0; i < NUM; i++)
        Xlast[i] = X[i];	// make a working copy

    StateEq(X, U, K1);	// k1 = f(x,u)

    for (i = 0; i < NUM; i++)
        X[i] = Xlast[i] + dT2 * K1[i];
    StateEq(X, U, K2);	// k2 = f(x+0.5*dT*k1,u)

    for (i = 0; i < NUM; i++)
        X[i] = Xlast[i] + dT2 * K2[i];

    StateEq(X, U, K3);	// k3 = f(x+0.5*dT*k2,u)

    for (i = 0; i < NUM; i++)
        X[i] = Xlast[i] + dT * K3[i];

    StateEq(X, U, K4);	// k4 = f(x+dT*k3,u)

    // Xnew  = X + dT*(k1+2*k2+2*k3+k4)/6
    for (i = 0; i < NUM; i++)
        X[i] = Xlast[i] + dT * (K1[i] + 2 * K2[i] + 2 * K3[i] + K4[i]) / 6;
}

//  ***  StateEq ********
//
//  State Variables = [Quaternion]
//  Deterministic Inputs = [AngularVel]
//  Disturbance Noise = [GyroNoise AccelNoise GyroRandomWalkNoise NO-AccelRandomWalkNoise]
//
//  Notes:
//  AngularVel  in body frame
//  Xdot is output of StateEq()
//  ************************************************

void StateEq(float X[NUM], float U[NUM], float Xdot[NUM])
{
    float wx, wy, wz, q0, q1, q2, q3;

    wx = U[1];
    wy = U[2];
    wz = U[3];

    q0 = X[0];
    q1 = X[1];
    q2 = X[2];
    q3 = X[3];

    // qdot = Q*w
    Xdot[0] = (-q1 * wx - q2 * wy - q3 * wz) / 2;
    Xdot[1] = ( q0 * wx - q3 * wy + q2 * wz) / 2;
    Xdot[2] = ( q3 * wx + q0 * wy - q1 * wz) / 2;
    Xdot[3] = (-q2 * wx + q1 * wy + q0 * wz) / 2;
}

Makefile

#
# 'make depend' uses makedepend to automatically generate dependencies 
#               (dependencies are added to end of Makefile)
# 'make'        build executable file 'quat'
# 'make clean'  removes all .o and executable files
#

# define the C compiler to use
CC = gcc

# define any compile-time flags
CFLAGS = -Wall -g -std=c99

# define any directories containing header files other than /usr/include
#
INCLUDES = -Iinclude

# define library paths in addition to /usr/lib
#   if I wanted to include libraries not in /usr/lib I'd specify
#   their path using -Lpath, something like:
LFLAGS = -Llib

# define any libraries to link into executable:
#   if I want to link in libraries (libx.so or libx.a) I use the -llibname 
#   option, something like (this will link in libmylib.so and libm.so:
LIBS = -lm

# define the C source files
SRCS = quat.c

# define the C object files 
#
# This uses Suffix Replacement within a macro:
#   $(name:string1=string2)
#         For each word in 'name' replace 'string1' with 'string2'
# Below we are replacing the suffix .c of all words in the macro SRCS
# with the .o suffix
#
OBJS = $(SRCS:.c=.o)

# define the executable file 
TARGET = quat

#
# The following part of the makefile is generic; it can be used to 
# build any executable just by changing the definitions above and by
# deleting dependencies appended to the file from 'make depend'
#

.PHONY: depend clean

all:    $(TARGET)
	@echo  Simple compiler named $(TARGET) has been compiled

$(TARGET): $(OBJS) 
	$(CC) $(CFLAGS) $(INCLUDES) -o $(TARGET) $(OBJS) $(LFLAGS) $(LIBS)

# this is a suffix replacement rule for building .o's from .c's
# it uses automatic variables $<: the name of the prerequisite of
# the rule(a .c file) and $@: the name of the target of the rule (a .o file) 
# (see the gnu make manual section about automatic variables)
.c.o:
	$(CC) $(CFLAGS) $(INCLUDES) -c $<  -o $@

clean:
	$(RM) *.o *~ $(TARGET)

depend: $(SRCS)
	makedepend $(INCLUDES) $^

# DO NOT DELETE THIS LINE -- make depend needs it


執行結果

圖形介面 3D-Cube :

cube.c

/****	Traditional first 3D program: spinning cube
	Written by Hugh Fisher				****/

#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include <GL/glut.h>

#include "utility.h"
#include "glUtils.h"

#define Near		0.5
#define Far		20.0

#define ESC		27

#define cmdRed		1
#define cmdGreen	2
#define cmdBlue 	3
#define cmdExit		99

static int	WinWidth, WinHeight;
static RGBA	CubeColor;
static int	AppMenu;
static GLfloat	Spin;
static GLfloat	ViewPoint[3];

/****		Cube in points-polygons (polyhedron) form	****/

static GLfloat Verts[8][3] = {
    { -0.5,  0.5, -0.5 },	/* 0 left top rear */
    {  0.5,  0.5, -0.5 },	/* 1 right top rear */
    {  0.5, -0.5, -0.5 },	/* 2 right bottom rear */
    { -0.5, -0.5, -0.5 },	/* 3 left bottom rear */
    { -0.5,  0.5,  0.5 },	/* 4 left top front */
    {  0.5,  0.5,  0.5 },	/* 5 right top front */
    {  0.5, -0.5,  0.5 },	/* 6 right bottom front */
    { -0.5, -0.5,  0.5 }	/* 7 left bottom front */
};

static GLuint Faces[] = {
    4, 5, 6, 7,	/* front */
    5, 1, 2, 6,	/* right */
    0, 4, 7, 3,	/* left */
    4, 0, 1, 5,	/* top */
    7, 6, 2, 3,	/* bottom */
    1, 0, 3, 2	/* rear */
};

static void drawCube()
{
    int i;

    glPointSize(5.0f);
    glLineWidth(5.0f);

    /* Draw cube in traditional OpenGL style */
    glBegin(GL_QUADS);
    for (i = 0; i < 6 * 4; i++)
        glVertex3fv(Verts[Faces[i]]);
    glEnd();
}

#if 0
static void arrayCube()
{
    /* Modern version using vertex arrays. Exactly the same effect
       as above, but only 2 OpenGL calls instead of 26. */

    /* Vertices are 3 floating point values each (XYZ), tightly
       packed in array. Array size is not specified nor checked!
       (Except by your program crashing if you get it wrong.) */
    glVertexPointer(3, GL_FLOAT, 0, Verts);

    /* Draw quads, using N vertices in total, in the order given
       by an array of ints, from the vertex array specified earlier. */
    glDrawElements(GL_QUADS, 6 * 4, GL_UNSIGNED_INT, Faces);
}
#endif

/****		Window events		****/

static void setProjection()
{
    GLfloat aspect;

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    aspect = (float)WinWidth / (float)WinHeight;
    gluPerspective(30.0, aspect, Near, Far); // 60.0
    /* Back to normal */
    glMatrixMode(GL_MODELVIEW);
}

static void setViewPoint()
{
    glLoadIdentity();
    gluLookAt(ViewPoint[0], ViewPoint[1], ViewPoint[2],
	      0.0, 0.0, 0.0,
	      0.0, 1.0, 0.0);
}

static void drawWorld()
{
    glPushMatrix();
    glRotatef(Spin, 1.0, 0.0, 0.0);
    //glRotatef(Spin, 0.0, 1.0, 0.0);
    //glRotatef(Spin, 0.0, 0.0, 1.0);
    glScalef(0.5, 0.5, 0.5);
    glColor3fv(CubeColor);
    drawCube();
    /* arrayCube() */
    glPopMatrix();
}

static void display()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    setProjection();
    setViewPoint();
    drawWorld();

    /* Check everything OK and update screen */
    CheckGL();
    glutSwapBuffers();
}

static void resize(int width, int height)
{
    /* Save for event handlers */
    WinWidth  = width;
    WinHeight = height;

    /* Reset view in window. */
    glViewport(0, 0, WinWidth, WinHeight);
}


/****		User events		****/


static void menuChoice(int item)
{
    switch (item) {
        case cmdRed:
            SetColor(CubeColor, 1, 0, 0);
            break;
        case cmdGreen:
            SetColor(CubeColor, 0, 1, 0);
            break;
	case cmdBlue:
            SetColor(CubeColor, 0, 0, 1);
            break;
        case cmdExit:
            exit(0);
            break;
        default:
            break;
    }
}


/* In most GUI systems we would write just one event handler
   for all kinds of keystrokes. In GLUT, we need one for the
   standard ASCII keys and one for special cursor or function
   style keys that vary from system to system. Because the
   GLUT special key code range overlaps with ASCII lowercase,
   it isn't safe to use the same function for both.        */

static void asciiKey(unsigned char key, int x, int y)
{
    if (key == ESC)
        menuChoice(cmdExit);
}

static void specialKey(int key, int x, int y)
{
    /* Nothing yet */
}


/****		Startup			****/

static void initGraphics(void)
{
    /* Black background */
    glClearColor(0, 0, 0, 0);
    /* Wireframe mode */
    glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);

    /* Needed for vertex arrays */
    glEnableClientState(GL_VERTEX_ARRAY);

    /* Popup menu attached to right mouse button */
    AppMenu = glutCreateMenu(menuChoice);
    glutSetMenu(AppMenu);
    glutAddMenuEntry("R", cmdRed);
    glutAddMenuEntry("G", cmdGreen);
    glutAddMenuEntry("B", cmdBlue);
    glutAddMenuEntry("----", 0);
    glutAddMenuEntry("Exit", cmdExit);
    glutAttachMenu(GLUT_RIGHT_BUTTON);

    /* Start values */
    Spin = 0.0;
    ViewPoint[0] = 0.0;
    ViewPoint[1] = 0.5;
    ViewPoint[2] = 2.0;

    menuChoice(cmdGreen);
}


/****		Main control		****/


static void spinDisplay(void)
{
    Spin += 1;
    if (Spin >= 360.0)
        Spin -= 360.0;
    glutPostRedisplay();
}

int main(int argc, char * argv[])
{
    glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_DEPTH | GLUT_RGB);

    glutInitWindowSize(500, 500);
    glutInitWindowPosition(100, 75);
    glutCreateWindow("Cube (CCTSAO1008)");

    initGraphics();

    glutDisplayFunc(display);
    glutReshapeFunc(resize);

    glutKeyboardFunc(asciiKey);
    glutSpecialFunc(specialKey);

    glutIdleFunc(spinDisplay);

    glutMainLoop();
    /* Should never get here, but keeps compiler happy */
    return 0;
}

Makefile

## Linux
CC      = gcc
CFLAGS  = -I../util -DGL_GLEXT_PROTOTYPES -Wall
LDFLAGS = 
# Older systems might need -lXi -lXmu
GLIBS   = -lglut -lGLU -lGL -lX11 -lm
 

OBJS = \
	../util/utility.o	\
	../util/glUtils.o	\

TARGET = cube
 
$(TARGET): $(TARGET).c $(OBJS)
	/bin/rm -f $@
	$(CC) $(CFLAGS) -o $@ $(TARGET).c $(OBJS) $(LDFLAGS) $(GLIBS)
 

clean:
	/bin/rm -f *.o $(TARGET)


cube.o: cube.c

參考資料 :

1. OpenGL SuperBible   http://www.openglsuperbible.com

2. 四元數更新   https://qcopter.googlecode.com/files/推導_四元數.pdf

© 著作权归作者所有

共有 人打赏支持
cctsao1008
粉丝 11
博文 5
码字总数 7465
作品 0
台北
高级程序员
私信 提问
加载中

评论(2)

B
Burrito
安安, TMR Project 還在執行嗎?看了幾篇你的文章,正好自己也在作一樣的事情,希望可以交流一下.
maybe_lan
maybe_lan
这是要准备写ekf的节奏?这一步是predict step?需要加速度与磁场的测量来进行correct step? 小白不懂,期待大神杰作(^_^)
在xcode中使用openGL一:程序框架搭建

OpengGL中的数据类型,跟C中很多是相似的,看名字就可以了,特殊的在下面 GLsizeiptr GLintptr GLsyn GLclampf GLbitfield 32位 GLshort shorts[10];//short数组 GLdouble *doubles[10];//10...

openlab
2013/11/06
0
1
Metal入门教程(八)Metal与OpenGL ES交互

前言 Metal入门教程(一)图片绘制 Metal入门教程(二)三维变换 Metal入门教程(三)摄像头采集渲染 Metal入门教程(四)灰度计算 Metal入门教程(五)视频渲染 Metal入门教程(六)边界检测...

落影loyinglin
2018/08/10
0
0
轻量级高品质视频播放器 MPV 0.27.0 发布

MPV 0.27.0 已发布,MPV 是一款基于 mplayer2 和 MPlayer 的轻量级高品质视频播放器。MPV 基于 OpenGL 视频输出,支持视频缩放、高质量算法、色彩管理、帧定时、插值、HDR 等功能。同时,利用...

王练
2017/09/16
1K
2
OpenGL学习入门之VS2010环境配置 [转]

OpenGL开发环境简介    基于OpenGL标准开发的应用程序运行时需有动态链接库OpenGL32.DLL、Glu32.DLL,这两个文件在安装Windows NT时已自动装载到C:WINDOWSSYSTEM32目录下(这里假定用户将W...

IMGTN
2012/07/24
0
0
Android OpenGL es 纹理坐标设定与贴图规则

当opengl对一个四方形进行贴图时,会定义纹理贴图坐标,一串数组,相信初学openggl es者看到后会很头疼,不知道写得是什么东西。现在就将我的研究成果与大家分享下! 当纹理映射启动后绘图时...

长平狐
2012/08/29
2.5K
0

没有更多内容

加载失败,请刷新页面

加载更多

Android 保活

1.Android不间断上报位置信息-应用进程防杀指南 2.Android锁屏无法继续定位问题 3.微信 Android 客户端后台保活经验分享

IT追寻者
6分钟前
0
0
基于Kubernetes的Spark集群部署实践

Spark是新一代分布式内存计算框架,Apache开源的顶级项目。相比于Hadoop Map-Reduce计算框架,Spark将中间计算结果保留在内存中,速度提升10~100倍;同时它还提供更丰富的算子,采用弹性分布...

hblt-j
6分钟前
1
0
NTP服务搭建

NTP服务搭建 如果是单独安装这个服务,请直接开始即可。如果是为了解决hadoop集群的时针偏差问题,配置ntp服务时,务必先关闭chd的相关服务。 一、准备环境 1、操作系统 CentOS7操作系统,准...

星汉
8分钟前
0
0
SPring AOP(面向切面编程)

AOP(面向切面编程) AOP是OOP(面向对象编程)的延续,但是它和面向对象的纵向编程不同,它是一个横向的切面式的编程。可以理解为oop就是一根柱子,如果需要就继续往上加长,而aop则是在需要...

MrBoyce
8分钟前
1
0
高性能Mysql:复制

1 复制概述 Mysql内建的复制功能是构建大型,高性能应用程序的基础。将Mysql的数据分布到多个系统上去,这种分布的机制,是通过将Mysql的某一台主机的数据复制到其它主机(slaves)上,并重新...

watermelon11
11分钟前
1
0

没有更多内容

加载失败,请刷新页面

加载更多

返回顶部
顶部