ACCIS/accis_step.cpp at main · SIOSlab/ACCIS · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
#include "accis.hpp"

#include "angles.hpp"
#include "format.hpp"
#include "roam.h"
#include "sifter.hpp"

#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>

#include <iostream>
#include <string>

void accis_sat::step() {

    // Get current time
    double t = times.back();

    // Get true state
    sat_state x_tru = states_tru.back();

    // Print true altitude
    if (step_no % 100 == 0) {

        const double tol = 1.0E-6;

        double lat, lon, alt;

        eci2ll_(&t, x_tru.x(), &tol, &lat, &lon, &alt);

        std::cout << "Altitude of Satellite " << sat_id << ": "
            << alt << std::endl;

    }

    // Get current state estimate distribution
    filter::dist dist_x = states_est.back();

    // Update state estimate -- GPS
    if (step_no % cadence_gps == 0) {

        vec<> z = h_gps.H(t, x_tru.X, rnd);

        dist_x = filt->update(t, z, dist_x, gps_err, h_gps);

    }

    // Update state estimate -- Star tracker
    if (step_no % cadence_str == 0) {

        vec<> z = h_str.H(t, x_tru.X, rnd);

        dist_x = filt->update(t, z, dist_x, str_err, h_str);

    }

    // Update state estimate -- Gyroscope
    if (step_no % cadence_gyr == 0) {

        vec<> z = h_gyr.H(t, x_tru.X, rnd);

        dist_x = filt->update(t, z, dist_x, gyr_err, h_gyr);

    }

    // Imaging & cross-calibration
    if (cadence_img != 0 && step_no % cadence_img == 0) {

        // Get image
        cv::Mat image = cam.real_image(t, x_tru);

        // Percentage of black pixels
        cv::Mat gray;
        cv::cvtColor(image, gray, cv::COLOR_RGB2GRAY);
        double blp = 100.0 - (100.0 * cv::countNonZero(gray)) / gray.total();

        // Save image
        cv::imwrite("images/pic_sat_" + int2str0(sat_id, 3) + "_" +
            int2str0(step_no, 6) + ".png", image);

        // Process image
        if (blp <= max_blp) {

            // Estimated state at image generation
            sat_state x_img;
            x_img.X = dist_x.mean;

            // Populate transmission tr_last
            tr_last.sat_id = sat_id;
            tr_last.step = step_no;
            tr_last.t = t;
            tr_last.sift = sifter::sift(t, x_img, image, cc.num_sift_pts);
            tr_last.dist_x = dist_x;

            // Run image-based cross-calibration
            dist_x = cc.run(tr_last, *filt);

        }

    }

    // Store updated state estimate
    states_est.back() = dist_x;

    // Set attitude control torque
    sat_state x_est;
    x_est.X = dist_x.mean;
    vec<3> tc = att_ctrl.tau(x_est.qb(), x_est.w(), t);
    roamps_("TC", tc.data());
    roamps_("JP", J.data());

    // Get pointing error
    vec<2> att_err_k;
    att_err_k.x() = rad2deg(x_tru.qb().angularDistance(att_ctrl.qct(t)));
    att_err_k.y() = rad2deg((x_tru.w() - att_ctrl.wc).norm());
    att_err.push_back(att_err_k);

    // Update step
    step_no++;

    // Get next time
    double t_next = step_no * dt;
    times.push_back(t_next);

    // Propagate true state
    sat_state x_tru_next;
    x_tru_next.X = dyn_tru.propagate_random(t, t_next, x_tru.X, rnd);
    states_tru.push_back(x_tru_next);

    // Predict state distribution
    filter::dist dist_x_next = filt->predict(t, t_next,
            dist_x, dist_w, dyn_tru);
    states_est.push_back(dist_x_next);

}