ahrs-ekf-template.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0194fbd8",
   "metadata": {},
   "outputs": [],
   "source": [
    "import json\n",
    "with open('test.txt') as f:\n",
    "    r = []\n",
    "    for l in f:\n",
    "        try:\n",
    "            r.append(json.loads(l))\n",
    "        except BaseException:\n",
    "            pass\n",
    "r[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4972f39c",
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "plt.rcParams[\"figure.figsize\"] = (20, 10)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6b3473ec",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy\n",
    "gyro = numpy.array([i[2] for i in r])\n",
    "accel = numpy.array([i[1] for i in r])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "85ad7f4b",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(gyro[:, :])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4f4e8b16",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(accel)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "02d5fef6",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(gyro[:, 1] * 0.010)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8e4e7d5c",
   "metadata": {},
   "outputs": [],
   "source": [
    "a = numpy.array([0., 0., 0.])\n",
    "agyro = []\n",
    "for g in gyro:\n",
    "    agyro.append(a + g * 0.010)\n",
    "    a += g * 0.010\n",
    "agyro = numpy.array(agyro)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "baef156f",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot(numpy.degrees(agyro[:, 1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2650bbce",
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4121c2c4",
   "metadata": {},
   "outputs": [],
   "source": [
    "def yrp(q):\n",
    "    srcp = 2 * (q[0] * q[1] + q[2] * q[3])\n",
    "    crcp = 1 - 2 * (q[1] * q[1] + q[2] * q[2])\n",
    "    roll = np.arctan2(srcp, crcp)\n",
    "    \n",
    "    sp = 2 * (q[0] * q[2] - q[3] * q[1])\n",
    "    if sp >= 1:\n",
    "        pitch = 3.14159/2\n",
    "    elif sp <= -1:\n",
    "        pitch = -3.14159/2\n",
    "    else:\n",
    "        pitch = np.arcsin(sp)\n",
    "    \n",
    "    sycp = 2 * (q[0] * q[3] + q[1] * q[2])\n",
    "    cycp = 1 - 2 * (q[2] * q[2] + q[3] * q[3])\n",
    "    yaw = np.arctan2(sycp, cycp)\n",
    "    \n",
    "    return yaw, roll, pitch"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a8ca5d89",
   "metadata": {},
   "outputs": [],
   "source": [
    "quat = [i[4][0][:4] for i in r]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "04438e30",
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "plt.plot(quat)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ffcf81f7",
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "plt.plot(numpy.array(list(map(yrp, quat)))[:, 0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3d3efd09",
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "plt.plot(numpy.degrees((numpy.array(list(map(yrp, quat)))[:, 1])))\n",
    "print(numpy.average(numpy.array(list(map(yrp, quat)))[100:200, 1]))\n",
    "print(numpy.average(numpy.array(list(map(yrp, quat)))[650:750, 1]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "da163f9b",
   "metadata": {},
   "outputs": [],
   "source": [
    "n = (numpy.min(((numpy.array(list(map(yrp, quat)))[200:500, 1])+numpy.pi/2)%numpy.pi))\n",
    "m = (numpy.max(((numpy.array(list(map(yrp, quat)))[200:500, 1])+numpy.pi/2)%numpy.pi))\n",
    "print(numpy.degrees(n), numpy.degrees(m), numpy.degrees(m-n))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e50d3678",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot((numpy.array(list(map(yrp, quat)))[:, 2]+numpy.pi/2)%numpy.pi-numpy.pi/2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ee527d75",
   "metadata": {},
   "outputs": [],
   "source": [
    "Gbase = numpy.average([numpy.linalg.norm(v) for v in accel[:15]])\n",
    "Gbase"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c387359f",
   "metadata": {},
   "outputs": [],
   "source": [
    "plt.plot([[*i[3], numpy.linalg.norm(i[3])] for i in r])\n",
    "plt.legend([\"X\", \"Y\", \"Z\", \"norm\"])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}